├── .github └── workflows │ ├── converage.yaml │ ├── python-package.yml │ └── python-publish.yml ├── .gitignore ├── .readthedocs.yaml ├── CITATION.cff ├── LICENSE ├── README.md ├── changelog.md ├── doc ├── Makefile ├── make.bat ├── requirements.txt └── source │ ├── _static │ ├── my_theme.css │ └── switcher.json │ ├── _templates │ └── version.html │ ├── api │ ├── irsim.env.rst │ ├── irsim.lib.rst │ ├── irsim.rst │ ├── irsim.util.rst │ ├── irsim.world.rst │ └── modules.rst │ ├── cave.png │ ├── conf.py │ ├── custom_behavior_methods.py │ ├── get_started │ ├── index.rst │ ├── install.rst │ └── quick_start.md │ ├── index.rst │ ├── usage │ ├── configure_behavior.md │ ├── configure_dynamic_random_env.md │ ├── configure_grid_map.md │ ├── configure_keyboard_control.md │ ├── configure_robots_obstacles.md │ ├── configure_sensor.md │ ├── gif │ │ ├── 3d_plot.gif │ │ ├── custom_behavior.gif │ │ ├── fov.gif │ │ ├── grid_map.gif │ │ ├── keyboard.gif │ │ ├── lidar2d.gif │ │ ├── multi_objects.gif │ │ ├── noise.gif │ │ ├── random_obstacles.gif │ │ ├── robot_obstacle.gif │ │ └── rvo.gif │ ├── index.rst │ ├── make_environment.md │ └── save_animation.md │ └── yaml_config │ ├── configuration.md │ └── index.rst ├── irsim ├── __init__.py ├── env │ ├── __init__.py │ ├── env_base.py │ ├── env_base3d.py │ ├── env_config.py │ ├── env_logger.py │ ├── env_plot.py │ └── env_plot3d.py ├── global_param │ ├── env_param.py │ ├── path_param.py │ └── world_param.py ├── lib │ ├── __init__.py │ ├── algorithm │ │ ├── generation.py │ │ ├── kinematics.py │ │ └── rvo.py │ ├── behavior │ │ ├── behavior.py │ │ ├── behavior_methods.py │ │ └── behavior_registry.py │ ├── handler │ │ ├── geometry_handler.py │ │ └── kinematics_handler.py │ └── path_planners │ │ ├── __init__.py │ │ ├── a_star.py │ │ ├── probabilistic_road_map.py │ │ ├── rrt.py │ │ └── rrt_star.py ├── usage │ ├── 01empty_world │ │ ├── empty_world.py │ │ └── empty_world.yaml │ ├── 02robot_world │ │ ├── car_world.yaml │ │ ├── robot_omni_world.yaml │ │ ├── robot_polygon_world.yaml │ │ ├── robot_world.py │ │ └── robot_world.yaml │ ├── 03obstacle_world │ │ ├── obstacle_world.py │ │ └── obstacle_world.yaml │ ├── 04collision_world │ │ ├── collision_world.py │ │ └── collision_world.yaml │ ├── 05lidar_world │ │ ├── lidar_world.py │ │ ├── lidar_world.yaml │ │ ├── lidar_world_laser_color.py │ │ ├── lidar_world_laser_color.yaml │ │ └── lidar_world_noise.yaml │ ├── 06multi_objects_world │ │ ├── multi_objects_large.py │ │ ├── multi_objects_large.yaml │ │ ├── multi_objects_world.py │ │ └── multi_objects_world.yaml │ ├── 07render_world │ │ ├── render.py │ │ ├── render.yaml │ │ └── render_save_figure.py │ ├── 08random_obstacle │ │ ├── dynamic_random.py │ │ ├── dynamic_random.yaml │ │ ├── random_obstacle.py │ │ └── random_obstacle.yaml │ ├── 09keyboard_control │ │ ├── keyboard_control.py │ │ └── keyboard_control.yaml │ ├── 10grid_map │ │ ├── grid_map.py │ │ ├── grid_map.yaml │ │ ├── grid_map_hm3d.py │ │ └── grid_map_hm3d.yaml │ ├── 11collision_avoidance │ │ ├── collision_avoidance.py │ │ ├── collision_avoidance.yaml │ │ ├── collision_avoidance_large.py │ │ └── collision_avoidance_large.yaml │ ├── 12dynamic_obstacle │ │ ├── dynamic_obstacle.py │ │ └── dynamic_obstacle.yaml │ ├── 13custom_behavior │ │ ├── custom_behavior.py │ │ ├── custom_behavior.yaml │ │ └── custom_behavior_methods.py │ ├── 14world_3d_plot │ │ ├── car_world.yaml │ │ ├── robot_omni_world.yaml │ │ ├── robot_polygon_world.yaml │ │ ├── robot_world_3d.py │ │ └── robot_world_3d.yaml │ ├── 15fov_world │ │ ├── fov_world.py │ │ ├── fov_world.yaml │ │ ├── fov_world_multi.py │ │ └── fov_world_multi.yaml │ └── 16noise_world │ │ ├── noise_world.py │ │ └── noise_world.yaml ├── util │ └── util.py ├── version.py └── world │ ├── __init__.py │ ├── arm_base.py │ ├── description │ ├── car_blue.png │ ├── car_green.png │ ├── car_red.png │ ├── diff_robot0.png │ └── diff_robot1.png │ ├── map │ ├── __init__.py │ ├── binary_map_generator_hm3d │ │ ├── README.md │ │ ├── binary_map_generator.py │ │ ├── hm3d_1.png │ │ ├── hm3d_2.png │ │ ├── hm3d_3.png │ │ ├── hm3d_4.png │ │ ├── hm3d_5.png │ │ ├── hm3d_6.png │ │ ├── hm3d_7.png │ │ ├── hm3d_8.png │ │ └── hm3d_9.png │ ├── cave.png │ └── obstacle_map.py │ ├── object_base.py │ ├── object_base_3d.py │ ├── object_factory.py │ ├── obstacles │ ├── obstacle_acker.py │ ├── obstacle_diff.py │ ├── obstacle_omni.py │ └── obstacle_static.py │ ├── robots │ ├── robot_acker.py │ ├── robot_diff.py │ ├── robot_omni.py │ └── robot_rigid3d.py │ ├── sensors │ ├── lidar2d.py │ └── sensor_factory.py │ ├── world.py │ └── world3d.py ├── pyproject.toml ├── requirements ├── requirements_py310.txt ├── requirements_py311.txt ├── requirements_py312.txt ├── requirements_py313.txt └── requirements_py39.txt ├── tests ├── cave.png ├── custom_behavior.py ├── custom_behavior.yaml ├── custom_behavior_methods.py ├── test.md ├── test_all_objects.py ├── test_all_objects.yaml ├── test_all_objects_3d.py ├── test_collision_avoidance.yaml ├── test_collision_world.yaml ├── test_fov_world.yaml ├── test_grid_map.yaml ├── test_keyboard_control.yaml ├── test_kinematics.py ├── test_multi_objects_world.yaml ├── test_path_planners.py ├── test_random_goals.py ├── test_render.yaml └── test_util.py ├── todo_list.md └── uv.lock /.github/workflows/converage.yaml: -------------------------------------------------------------------------------- 1 | name: API workflow 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | runs-on: ubuntu-latest 8 | name: Test python API 9 | steps: 10 | - uses: actions/checkout@v4 11 | - name: Install requirements 12 | run: | 13 | sudo apt-get update 14 | sudo apt-get install python3-opengl xvfb python3-tk 15 | python3 -m pip install --upgrade pip 16 | pip install -e . --user 17 | 18 | - name: Install pytest 19 | run: pip install pytest pytest-cov pynput 20 | - name: Run tests and collect coverage 21 | run: xvfb-run -s "-screen 0 1024x768x24" pytest --cov . --cov-report=xml --cov-report=html 22 | - name: Upload coverage reports to Codecov 23 | run: | 24 | # Replace `linux` below with the appropriate OS 25 | # Options are `alpine`, `linux`, `macos`, `windows` 26 | curl -Os https://cli.codecov.io/latest/linux/codecov 27 | chmod +x codecov 28 | ./codecov --verbose upload-process --fail-on-error -t ${{ secrets.CODECOV_TOKEN }} -n 'service'-${{ github.run_id }} -F service -f coverage-service.xml -------------------------------------------------------------------------------- /.github/workflows/python-package.yml: -------------------------------------------------------------------------------- 1 | # This workflow will install Python dependencies, run tests and lint with a variety of Python versions 2 | # For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python 3 | 4 | name: Python package 5 | 6 | on: 7 | push: 8 | branches: [ "dev"] 9 | pull_request: 10 | branches: [ "dev" ] 11 | release: 12 | types: [published] 13 | 14 | 15 | jobs: 16 | build: 17 | 18 | runs-on: ubuntu-latest 19 | strategy: 20 | fail-fast: false 21 | matrix: 22 | python-version: ["3.9", "3.10", "3.11", "3.12", "3.13"] 23 | 24 | steps: 25 | - uses: actions/checkout@v4 26 | - name: Set up Python ${{ matrix.python-version }} 27 | uses: actions/setup-python@v3 28 | with: 29 | python-version: ${{ matrix.python-version }} 30 | - name: Install dependencies 31 | run: | 32 | python -m pip install --upgrade pip 33 | python -m pip install flake8 pytest 34 | python -m pip install ir-sim[all] --upgrade 35 | if [ -f requirements.txt ]; then pip install -r requirements.txt; fi 36 | - name: Lint with flake8 37 | run: | 38 | # stop the build if there are Python syntax errors or undefined names 39 | flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics 40 | # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide 41 | flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics 42 | - name: Test with pytest 43 | run: | 44 | xvfb-run -s "-screen 0 1024x768x24" pytest 45 | -------------------------------------------------------------------------------- /.github/workflows/python-publish.yml: -------------------------------------------------------------------------------- 1 | # This workflow will upload a Python Package using Twine when a release is created 2 | # For more information see: https://docs.github.com/en/actions/automating-builds-and-tests/building-and-testing-python#publishing-to-package-registries 3 | 4 | # This workflow uses actions that are not certified by GitHub. 5 | # They are provided by a third-party and are governed by 6 | # separate terms of service, privacy policy, and support 7 | # documentation. 8 | 9 | name: Upload Python Package 10 | 11 | on: 12 | release: 13 | types: [published] 14 | 15 | permissions: 16 | contents: read 17 | 18 | jobs: 19 | deploy: 20 | 21 | runs-on: ubuntu-latest 22 | 23 | steps: 24 | - uses: actions/checkout@v4 25 | - name: Set up Python 26 | uses: actions/setup-python@v3 27 | with: 28 | python-version: '3.x' 29 | - name: Install dependencies 30 | run: | 31 | python -m pip install --upgrade pip 32 | pip install build 33 | - name: Build package 34 | run: python -m build 35 | - name: Publish package 36 | uses: pypa/gh-action-pypi-publish@27b31702a0e7fc50959f5ad993c78deac1bdfc29 37 | with: 38 | user: __token__ 39 | password: ${{ secrets.PYPI_API_TOKEN }} 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib64/ 18 | parts/ 19 | sdist/ 20 | var/ 21 | wheels/ 22 | pip-wheel-metadata/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | 53 | # Translations 54 | *.mo 55 | *.pot 56 | 57 | # Django stuff: 58 | *.log 59 | local_settings.py 60 | db.sqlite3 61 | db.sqlite3-journal 62 | 63 | # Flask stuff: 64 | instance/ 65 | .webassets-cache 66 | 67 | # Scrapy stuff: 68 | .scrapy 69 | 70 | # Sphinx documentation 71 | docs/_build/ 72 | 73 | # PyBuilder 74 | target/ 75 | 76 | # Jupyter Notebook 77 | .ipynb_checkpoints 78 | 79 | # IPython 80 | profile_default/ 81 | ipython_config.py 82 | 83 | # pyenv 84 | .python-version 85 | 86 | # pipenv 87 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 88 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 89 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 90 | # install all needed dependencies. 91 | #Pipfile.lock 92 | 93 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 94 | __pypackages__/ 95 | 96 | # Celery stuff 97 | celerybeat-schedule 98 | celerybeat.pid 99 | 100 | # SageMath parsed files 101 | *.sage.py 102 | 103 | # Environments 104 | .env 105 | .venv 106 | # env/ 107 | venv/ 108 | # ENV/ 109 | env.bak/ 110 | venv.bak/ 111 | 112 | # Spyder project settings 113 | .spyderproject 114 | .spyproject 115 | 116 | # Rope project settings 117 | .ropeproject 118 | 119 | # mkdocs documentation 120 | /site 121 | 122 | # mypy 123 | .mypy_cache/ 124 | .dmypy.json 125 | dmypy.json 126 | 127 | # Pyre type checker 128 | .pyre/ 129 | .vscode/settings.json 130 | 131 | #log 132 | *.log.* 133 | 134 | test.py 135 | 136 | .vscode/ 137 | *.code-workspace 138 | 139 | test/ 140 | test.yaml 141 | result.json 142 | test.html 143 | 144 | test_animation.gif 145 | test_animation.mp4 146 | -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | # Read the Docs configuration file for Sphinx projects 2 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 3 | 4 | # Required 5 | version: 2 6 | 7 | # Set the OS, Python version and other tools you might need 8 | build: 9 | os: ubuntu-22.04 10 | tools: 11 | python: "3.11" 12 | # You can also specify other tool versions: 13 | # nodejs: "20" 14 | # rust: "1.70" 15 | # golang: "1.20" 16 | 17 | 18 | # Build documentation in the "docs/" directory with Sphinx 19 | sphinx: 20 | builder: html 21 | configuration: doc/source/conf.py 22 | # You can configure Sphinx to use a different builder, for instance use the dirhtml builder for simpler URLs 23 | # builder: "dirhtml" 24 | # Fail on all warnings to avoid broken references 25 | # fail_on_warning: true 26 | 27 | # Optionally build your docs in additional formats such as PDF and ePub 28 | # formats: 29 | # - pdf 30 | # - epub 31 | 32 | # Optional but recommended, declare the Python requirements required 33 | # to build your documentation 34 | # See https://docs.readthedocs.io/en/stable/guides/reproducible-builds.html 35 | python: 36 | install: 37 | - requirements: doc/requirements.txt 38 | - method: pip 39 | path: . -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "Han" 5 | given-names: "Ruihua, et al." 6 | title: "IR-SIM: An open-source lightweight simulator for robot navigation, control, and learning" 7 | url: "https://github.com/hanruihua/ir-sim" 8 | preferred-citation: 9 | type: generic 10 | authors: 11 | - family-names: "Han" 12 | given-names: "Ruihua, et al." 13 | title: "IR-SIM: An open-source lightweight simulator for robot navigation, control, and learning" 14 | year: 2024 15 | url: "https://github.com/hanruihua/ir-sim" 16 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Ruihua Han 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /doc/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /doc/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=source 11 | set BUILDDIR=build 12 | 13 | %SPHINXBUILD% >NUL 2>NUL 14 | if errorlevel 9009 ( 15 | echo. 16 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 17 | echo.installed, then set the SPHINXBUILD environment variable to point 18 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 19 | echo.may add the Sphinx directory to PATH. 20 | echo. 21 | echo.If you don't have Sphinx installed, grab it from 22 | echo.https://www.sphinx-doc.org/ 23 | exit /b 1 24 | ) 25 | 26 | if "%1" == "" goto help 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /doc/requirements.txt: -------------------------------------------------------------------------------- 1 | sphinx 2 | sphinx_rtd_theme 3 | sphinx-autodoc-typehints 4 | sphinx_multiversion 5 | myst_parser 6 | importlib-metadata 7 | sphinx_copybutton 8 | pydata-sphinx-theme 9 | # sphinx.ext.viewcode 10 | # sphinx.ext.githubpages 11 | # sphinx.ext.napoleon 12 | 13 | -------------------------------------------------------------------------------- /doc/source/_static/my_theme.css: -------------------------------------------------------------------------------- 1 | .wy-nav-content { 2 | max-width: 1000px !important; 3 | } -------------------------------------------------------------------------------- /doc/source/_static/switcher.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "name": "2.2.6 (latest)", 4 | "version": "2.2.6", 5 | "url": "https://ir-sim.readthedocs.io/en/latest/" 6 | } 7 | ] -------------------------------------------------------------------------------- /doc/source/_templates/version.html: -------------------------------------------------------------------------------- 1 |
2 | 3 | Version: Current version here 4 | 5 | 6 |
7 | We list the other versions here 8 |
9 |
-------------------------------------------------------------------------------- /doc/source/api/irsim.env.rst: -------------------------------------------------------------------------------- 1 | env 2 | =================== 3 | 4 | irsim.env.env\_base 5 | ---------------------------- 6 | 7 | .. automodule:: irsim.env.env_base 8 | :members: 9 | :undoc-members: 10 | :show-inheritance: 11 | 12 | .. automodule:: irsim.env.env_base3d 13 | :members: 14 | :undoc-members: 15 | :show-inheritance: 16 | 17 | irsim.env.env\_logger 18 | ------------------------------ 19 | 20 | .. automodule:: irsim.env.env_logger 21 | :members: 22 | :undoc-members: 23 | :show-inheritance: 24 | 25 | irsim.env.env\_config 26 | ---------------------------- 27 | 28 | .. automodule:: irsim.env.env_config 29 | :members: 30 | :undoc-members: 31 | :show-inheritance: 32 | 33 | irsim.env.env\_plot 34 | ---------------------------- 35 | 36 | .. automodule:: irsim.env.env_plot 37 | :members: 38 | :undoc-members: 39 | :show-inheritance: 40 | 41 | 42 | -------------------------------------------------------------------------------- /doc/source/api/irsim.lib.rst: -------------------------------------------------------------------------------- 1 | lib 2 | =================== 3 | 4 | 5 | irsim.lib.behavior 6 | --------------------------- 7 | 8 | .. automodule:: irsim.lib.behavior.behavior 9 | :members: 10 | :undoc-members: 11 | :show-inheritance: 12 | 13 | irsim.lib.behavior\_methods 14 | ------------------------------ 15 | 16 | .. automodule:: irsim.lib.behavior.behavior_methods 17 | :members: 18 | :undoc-members: 19 | :show-inheritance: 20 | 21 | 22 | irsim.lib.handler 23 | ----------------------------- 24 | 25 | .. automodule:: irsim.lib.handler.geometry_handler 26 | :members: 27 | :undoc-members: 28 | :show-inheritance: 29 | 30 | .. automodule:: irsim.lib.handler.kinematics_handler 31 | :members: 32 | :undoc-members: 33 | :show-inheritance: 34 | 35 | 36 | irsim.lib.algorithm 37 | ----------------------------- 38 | 39 | .. automodule:: irsim.lib.algorithm.generation 40 | :members: 41 | :undoc-members: 42 | :show-inheritance: 43 | 44 | .. automodule:: irsim.lib.algorithm.kinematics 45 | :members: 46 | :undoc-members: 47 | :show-inheritance: 48 | 49 | .. automodule:: irsim.lib.algorithm.rvo 50 | :members: 51 | :undoc-members: 52 | :show-inheritance: 53 | 54 | 55 | -------------------------------------------------------------------------------- /doc/source/api/irsim.rst: -------------------------------------------------------------------------------- 1 | irsim 2 | =================== 3 | 4 | .. automodule:: irsim 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: -------------------------------------------------------------------------------- /doc/source/api/irsim.util.rst: -------------------------------------------------------------------------------- 1 | util 2 | =================== 3 | 4 | 5 | irsim.util 6 | --------------------------- 7 | 8 | .. automodule:: irsim.util.util 9 | :members: 10 | :undoc-members: 11 | :show-inheritance: 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /doc/source/api/irsim.world.rst: -------------------------------------------------------------------------------- 1 | world 2 | ===================== 3 | 4 | 5 | irsim.world 6 | -------------------------- 7 | 8 | .. automodule:: irsim.world.world 9 | :members: 10 | :undoc-members: 11 | :show-inheritance: 12 | 13 | 14 | irsim.world.object\_factory 15 | ------------------------------------ 16 | 17 | .. automodule:: irsim.world.object_factory 18 | :members: 19 | :undoc-members: 20 | :show-inheritance: 21 | 22 | .. automodule:: irsim.world.object_base 23 | :members: 24 | :undoc-members: 25 | :show-inheritance: 26 | 27 | 28 | irsim.world.sensor\_factory 29 | ------------------------------------ 30 | 31 | .. automodule:: irsim.world.sensors.lidar2d 32 | :members: 33 | :undoc-members: 34 | :show-inheritance: 35 | 36 | .. automodule:: irsim.world.sensors.sensor_factory 37 | :members: 38 | :undoc-members: 39 | :show-inheritance: 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /doc/source/api/modules.rst: -------------------------------------------------------------------------------- 1 | API Documentation 2 | ==================== 3 | 4 | .. toctree:: 5 | :maxdepth: 4 6 | 7 | irsim 8 | irsim.env 9 | irsim.lib 10 | irsim.world 11 | irsim.util -------------------------------------------------------------------------------- /doc/source/cave.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/cave.png -------------------------------------------------------------------------------- /doc/source/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | # 13 | # import os 14 | # import sys 15 | # sys.path.insert(0, os.path.abspath('.')) 16 | import os 17 | import sys 18 | from unittest.mock import MagicMock 19 | import irsim 20 | 21 | class Mock(MagicMock): 22 | @classmethod 23 | def __getattr__(cls, name): 24 | return MagicMock() 25 | 26 | MOCK_MODULES = ['pynput', 'loguru'] 27 | sys.modules.update((mod_name, Mock()) for mod_name in MOCK_MODULES) 28 | 29 | # Rest of your conf.py content 30 | 31 | # -- Project information ----------------------------------------------------- 32 | 33 | project = 'IR-SIM' 34 | copyright = '2024, Ruihua Han' 35 | author = 'Ruihua Han' 36 | 37 | # The full version, including alpha/beta/rc tags 38 | release = irsim.__version__ 39 | 40 | # print(os.path.abspath('../../')) 41 | # sys.path.insert(0, os.path.abspath('../../')) 42 | # sys.path.insert(0, os.path.abspath("../..")) 43 | # sys.path.insert(0, os.path.abspath("./")) 44 | # sys.path.insert(0, os.path.join(os.path.dirname((os.path.abspath('.')), 'irsim'))) 45 | 46 | # root_path = os.path.abspath(os.path.dirname(os.path.dirname(os.path.dirname(__file__)))) 47 | # sys.path.insert(0, os.path.dirname(__file__)) 48 | 49 | # sys.path.insert(0, os.path.abspath("../..")) 50 | # print(os.path.abspath('./')) 51 | # sys.path.insert(0, os.path.abspath('./')) 52 | # sys.path.insert(0, os.path.abspath('../src')) 53 | # sys.path.append(os.path.abspath('../../')) 54 | # sys.path.insert(0, os.path.abspath("..")) 55 | 56 | # -- General configuration --------------------------------------------------- 57 | 58 | # Add any Sphinx extension module names here, as strings. They can be 59 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 60 | # ones. 61 | extensions = ['sphinx.ext.autodoc', 'sphinx.ext.viewcode', 'sphinx.ext.napoleon', 'myst_parser', 'sphinx_multiversion', 'sphinx_copybutton' 62 | ] 63 | 64 | myst_enable_extensions = [ 65 | "amsmath", 66 | "deflist", 67 | "html_admonition", 68 | "html_image", 69 | "colon_fence", 70 | # Add other extensions as needed 71 | ] 72 | 73 | # Add any paths that contain templates here, relative to this directory. 74 | templates_path = ['_templates'] 75 | 76 | # List of patterns, relative to source directory, that match files and 77 | # directories to ignore when looking for source files. 78 | # This pattern also affects html_static_path and html_extra_path. 79 | exclude_patterns = [] 80 | 81 | # root_doc = 'irsim' 82 | # -- Options for HTML output ------------------------------------------------- 83 | 84 | # The theme to use for HTML and HTML Help pages. See the documentation for 85 | # a list of builtin themes. 86 | # 87 | # html_theme = 'alabaster' 88 | 89 | # Add any paths that contain custom static files (such as style sheets) here, 90 | # relative to this directory. They are copied after the builtin static files, 91 | # so a file named "default.css" will overwrite the builtin "default.css". 92 | html_static_path = ['_static'] 93 | 94 | # html_theme = "sphinx_rtd_theme" 95 | html_theme = "pydata_sphinx_theme" 96 | 97 | autodoc_member_order = 'bysource' 98 | 99 | source_suffix = { 100 | '.rst': 'restructuredtext', 101 | '.md': 'markdown', 102 | } 103 | 104 | # myst_enable_extensions = [ 105 | # "amsmath", 106 | # "attrs_inline", 107 | # "colon_fence", 108 | # "deflist", 109 | # "dollarmath", 110 | # "fieldlist", 111 | # "html_admonition", 112 | # "html_image", 113 | # "replacements", 114 | # "smartquotes", 115 | # "strikethrough", 116 | # "substitution", 117 | # "tasklist", 118 | # ] 119 | 120 | # json_url = "https://ir-sim.readthedocs.io/en/dev/_static/switcher.json" 121 | 122 | # html_theme_options = { 123 | # "switcher": { 124 | # "json_url": json_url, 125 | # "version_match": release, 126 | # }, 127 | # } 128 | 129 | 130 | def setup(app): 131 | app.add_css_file('my_theme.css') -------------------------------------------------------------------------------- /doc/source/custom_behavior_methods.py: -------------------------------------------------------------------------------- 1 | from irsim.lib import register_behavior 2 | from irsim.util.util import relative_position, WrapToPi 3 | import numpy as np 4 | 5 | 6 | @register_behavior("diff", "dash_custom") 7 | def beh_diff_dash(ego_object, objects, **kwargs): 8 | 9 | state = ego_object.state 10 | goal = ego_object.goal 11 | goal_threshold = ego_object.goal_threshold 12 | _, max_vel = ego_object.get_vel_range() 13 | angle_tolerance = kwargs.get("angle_tolerance", 0.1) 14 | 15 | behavior_vel = DiffDash2(state, goal, max_vel, goal_threshold, angle_tolerance) 16 | 17 | return behavior_vel 18 | 19 | 20 | def DiffDash2(state, goal, max_vel, goal_threshold=0.1, angle_tolerance=0.2): 21 | 22 | distance, radian = relative_position(state, goal) 23 | 24 | if distance < goal_threshold: 25 | return np.zeros((2, 1)) 26 | 27 | diff_radian = WrapToPi(radian - state[2, 0]) 28 | linear = max_vel[0, 0] * np.cos(diff_radian) 29 | 30 | if abs(diff_radian) < angle_tolerance: 31 | angular = 0 32 | else: 33 | angular = max_vel[1, 0] * np.sign(diff_radian) 34 | 35 | return np.array([[linear], [angular]]) 36 | -------------------------------------------------------------------------------- /doc/source/get_started/index.rst: -------------------------------------------------------------------------------- 1 | Get Started 2 | =============== 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | install 8 | quick_start 9 | 10 | -------------------------------------------------------------------------------- /doc/source/get_started/install.rst: -------------------------------------------------------------------------------- 1 | Install the package 2 | ================================= 3 | 4 | You can install the package using the pip: 5 | 6 | .. code-block:: bash 7 | 8 | pip install ir-sim 9 | 10 | This will install the package and core dependencies for the base environment. For other optional dependencies, you can install them separately like the following: 11 | 12 | **Optional dependencies:** 13 | 14 | For keyboard control, you can install the package with the following command: 15 | 16 | .. code-block:: bash 17 | 18 | pip install ir-sim[keyboard] 19 | 20 | **All dependencies:** 21 | 22 | To install all the optional dependencies. You can install the package with the following command: 23 | 24 | .. code-block:: bash 25 | 26 | pip install ir-sim[all] 27 | 28 | 29 | **Source code:** 30 | 31 | or you can install the package from the source code for development: 32 | 33 | .. code-block:: bash 34 | 35 | git clone https://github.com/hanruihua/ir-sim 36 | cd ir-sim 37 | pip install -e . 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /doc/source/get_started/quick_start.md: -------------------------------------------------------------------------------- 1 | Quick Start 2 | =============== 3 | 4 | To quickly start the simulation, you can use the following code snippet to run a simulation for a robot in a world. 5 | 6 | 1. Create a Python file and copy the following code snippet to run the simulation. 7 | 8 | ```python 9 | import irsim 10 | 11 | env = irsim.make('robot_world.yaml') # initialize the environment with the configuration file 12 | 13 | for i in range(300): # run the simulation for 300 steps 14 | 15 | env.step() # update the environment 16 | env.render() # render the environment 17 | 18 | if env.done(): 19 | break # check if the simulation is done 20 | 21 | env.end() # close the environment 22 | ``` 23 | 24 | 2. Create a configuration YAML file *robot_world.yaml* and copy the following configuration to the file. 25 | 26 | All the configurations are set in the YAML file. You can change the configurations in the YAML file to customize the simulation. The following is an example of the configuration file *robot_world.yaml*. 27 | 28 | ```yaml 29 | world: 30 | height: 10 # the height of the world 31 | width: 10 # the width of the world 32 | step_time: 0.1 # 10Hz to calculate each step 33 | sample_time: 0.1 # 10 Hz for render and data extraction 34 | offset: [0, 0] # the offset of the world on x and y 35 | 36 | robot: 37 | kinematics: {name: 'diff'} # kinematics of the robot, current name should be one of omni, diff, acker. If not set, this object will be static 38 | shape: {name: 'circle', radius: 0.2} # radius for circle shape 39 | state: [1, 1, 0] # x, y, theta, 2d position and orientation 40 | goal: [9, 9, 0] # x, y, theta, 2d position and orientation 41 | behavior: {name: 'dash'} # move toward the goal directly 42 | color: 'g' # green 43 | ``` -------------------------------------------------------------------------------- /doc/source/index.rst: -------------------------------------------------------------------------------- 1 | .. IR-SIM documentation master file, created by 2 | sphinx-quickstart on Tue Aug 6 00:59:39 2024. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to IR-SIM's documentation! 7 | ================================== 8 | 9 | **IR-SIM** is an open-source, lightweight robot simulator based on Python, designed for robotics navigation, control, and learning. This simulator provides a simple, user-friendly framework for simulating robots, sensors, and environments, facilitating the development and testing of robotics algorithms with minimal hardware requirements. 10 | 11 | **Features** 12 | 13 | - Simulate a wide range of robot platforms with diverse kinematics, sensors, and behaviors. 14 | - Quickly configure and customize simulation scenarios using straightforward YAML files, with no complex coding required. 15 | - Visualize simulation outcomes in real time for immediate feedback and analysis using a naive visualizer matplotlib. 16 | - Support collision detection and behavior control for each object in the simulation. 17 | The simple demonstrations of the simulator are shown below: 18 | 19 | .. image:: https://github.com/user-attachments/assets/5930b088-d400-4943-8ded-853c22eae75b 20 | :width: 49% 21 | :alt: Alternative text 22 | 23 | 24 | .. image:: https://github.com/user-attachments/assets/3257abc1-8bed-40d8-9b51-e5d90b06ee06 25 | :width: 49% 26 | :alt: Alternative text 27 | 28 | .. image:: https://github.com/user-attachments/assets/7aa809c2-3a44-4377-a22d-728b9dbdf8bc 29 | :width: 49% 30 | :alt: Alternative text 31 | 32 | .. image:: https://github.com/user-attachments/assets/1cc8a4a6-2f41-4bc9-bc59-a7faff443223 33 | :width: 49% 34 | :alt: Alternative text 35 | 36 | 37 | .. toctree:: 38 | :maxdepth: 2 39 | :caption: Getting Started: 40 | 41 | get_started/index 42 | 43 | .. toctree:: 44 | :maxdepth: 2 45 | :caption: User Guide: 46 | 47 | usage/index 48 | 49 | .. toctree:: 50 | :maxdepth: 2 51 | :caption: YAML Configuration Syntax: 52 | 53 | yaml_config/index 54 | 55 | 56 | .. toctree:: 57 | :maxdepth: 2 58 | :caption: API Documentation: 59 | 60 | api/modules 61 | 62 | 63 | Cases 64 | -------------- 65 | 66 | **Academic** 67 | 68 | - `rl-rvo-nav(RAL & ICRA2023) `_ 69 | 70 | - `RDA_planner(RAL & IROS2023) `_ 71 | 72 | - `NeuPAN(T-RO 2025) `_ 73 | 74 | **Deep Reinforcement Learning** 75 | 76 | - `DRL-robot-navigation-IR-SIM `_ 77 | - `AutoNavRL `_ 78 | 79 | Code Repository 80 | --------------- 81 | `IR-SIM `_ -------------------------------------------------------------------------------- /doc/source/usage/configure_behavior.md: -------------------------------------------------------------------------------- 1 | Configure behavior for objects 2 | ============================= 3 | 4 | Each object in the simulation can be assigned a behavior independently to simulate different scenarios. The behavior of the object can be configured by specifying the behavior parameters in the YAML configuration file. 5 | 6 | ## Behavior Configuration Parameters 7 | 8 | Currently, there are two built-in behaviors: `dash` and `rvo`. By default, the moving objects' behavior is static, which means the object will not move. You can set the behavior of the object to `dash` or `rvo` in the YAML configuration file. The `dash` behavior is a simple behavior that moves the object from its initial position to the goal position directly. The `rvo` behavior is a dynamic collision avoidance algorithm for multiple agents. The example of this behavior is shown below: 9 | 10 | python script: 11 | 12 | ```python 13 | import irsim 14 | 15 | env = irsim.make() 16 | 17 | for i in range(1000): 18 | 19 | env.step() 20 | env.render(0.05) 21 | 22 | if env.done(): 23 | break 24 | 25 | env.end() 26 | ``` 27 | 28 | YAML file: 29 | 30 | ```yaml 31 | world: 32 | height: 10 # the height of the world 33 | width: 10 # the width of the world 34 | 35 | robot: 36 | - number: 10 37 | distribution: {name: 'circle', radius: 4.0, center: [5, 5]} 38 | kinematics: {name: 'diff'} 39 | shape: 40 | - {name: 'circle', radius: 0.2} 41 | behavior: {name: 'rvo', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 1.0} 42 | vel_min: [-3, -3.0] 43 | vel_max: [3, 3.0] 44 | color: ['royalblue', 'red', 'green', 'orange', 'purple', 'yellow', 'cyan', 'magenta', 'lime', 'pink', 'brown'] 45 | arrive_mode: position 46 | goal_threshold: 0.15 47 | plot: 48 | show_trail: true 49 | show_goal: true 50 | trail_fill: true 51 | trail_alpha: 0.2 52 | show_trajectory: false 53 | ``` 54 | 55 | The demonstration of the `rvo` behavior is shown in the following figure: 56 | 57 | ```{image} gif/rvo.gif 58 | :alt: Select Parameters 59 | :width: 400px 60 | :align: center 61 | ``` 62 | 63 | ### Important Behavior Parameters Explained 64 | 65 | - **`name`:** The name of the behavior. The default behavior is `dash`. The `rvo` behavior is a local collision avoidance algorithm for multiple agents. 66 | - **`vxmax`:** The maximum velocity in the x direction. 67 | - **`vymax`:** The maximum velocity in the y direction. 68 | - **`acce`:** The acceleration of the object. 69 | - **`factor`:** The factor to adjust the collision penalty. 70 | 71 | Full list of behavior parameters can be found in the [YAML Configuration](../yaml_config/configuration/). 72 | 73 | 74 | ## Advanced Configuration for Custom Behavior 75 | 76 | ### Custom Behavior Function 77 | 78 | If you want to create a custom behavior for the object, you should first define your own behavior in a custom python script (e.g. `custom_behavior_methods.py`) as shown below: 79 | 80 | ```python 81 | from irsim.lib import register_behavior 82 | from irsim.util.util import relative_position, WrapToPi 83 | import numpy as np 84 | 85 | @register_behavior("diff", "dash_custom") 86 | def beh_diff_dash(ego_object, objects, **kwargs): 87 | 88 | state = ego_object.state 89 | goal = ego_object.goal 90 | goal_threshold = ego_object.goal_threshold 91 | _, max_vel = ego_object.get_vel_range() 92 | angle_tolerance = kwargs.get("angle_tolerance", 0.1) 93 | 94 | behavior_vel = DiffDash2(state, goal, max_vel, goal_threshold, angle_tolerance) 95 | 96 | return behavior_vel 97 | 98 | 99 | def DiffDash2(state, goal, max_vel, goal_threshold=0.1, angle_tolerance=0.2): 100 | 101 | distance, radian = relative_position(state, goal) 102 | 103 | if distance < goal_threshold: 104 | return np.zeros((2, 1)) 105 | 106 | diff_radian = WrapToPi(radian - state[2, 0]) 107 | linear = max_vel[0, 0] * np.cos(diff_radian) 108 | 109 | if abs(diff_radian) < angle_tolerance: 110 | angular = 0 111 | else: 112 | angular = max_vel[1, 0] * np.sign(diff_radian) 113 | 114 | return np.array([[linear], [angular]]) 115 | 116 | ``` 117 | 118 | The `ego_object` is the object that you want to control, and the `objects` is the list of all objects in the simulation. The custom behavior function should return the velocity of the object. You can obtain the state, goal, and other properties from the objects. `DiffDash2` is the custom behavior function that calculates the dash velocity of the object. 119 | 120 | :::{important} 121 | You must use the `@register_behavior` decorator to register the custom behavior. The first argument of the decorator is the name of the kinematics (`diff`, `acker`, `omni`), and the second argument is the name of the behavior used in YAML file. 122 | ::: 123 | 124 | ### Register Behavior 125 | 126 | The python script to run the simulation with the custom behavior is shown below: 127 | 128 | ```python 129 | import irsim 130 | 131 | env = irsim.make() 132 | env.load_behavior("custom_behavior_methods") 133 | 134 | for i in range(1000): 135 | 136 | env.step() 137 | env.render(0.01) 138 | 139 | if env.done(): 140 | break 141 | 142 | env.end(5) 143 | ``` 144 | 145 | `load_behavior` function loads the custom behavior methods from the python script. `custom_behavior_methods` is the name of the python script that contains the custom behavior function. 146 | 147 | :::{note} 148 | Please place the script with the name `custom_behavior_methods` in the same directory as the main python script. 149 | ::: 150 | 151 | ### Run Custom Behavior in the simulation 152 | 153 | Now, you can use the custom behavior `dash_custom` in the YAML configuration file as shown below. 154 | 155 | ```yaml 156 | world: 157 | height: 10 # the height of the world 158 | width: 10 # the width of the world 159 | 160 | robot: 161 | - number: 10 162 | distribution: {name: 'circle', radius: 4.0, center: [5, 5]} 163 | kinematics: {name: 'diff'} 164 | shape: 165 | - {name: 'circle', radius: 0.2} 166 | behavior: {name: 'dash_custom'} 167 | vel_min: [-3, -3.0] 168 | vel_max: [3, 3.0] 169 | color: ['royalblue', 'red', 'green', 'orange', 'purple', 'yellow', 'cyan', 'magenta', 'lime', 'pink', 'brown'] 170 | arrive_mode: position 171 | goal_threshold: 0.15 172 | plot: 173 | show_trail: true 174 | show_goal: true 175 | trail_fill: true 176 | trail_alpha: 0.2 177 | show_trajectory: false 178 | ``` 179 | 180 | You will see the custom behavior in the simulation. 181 | 182 | ```{image} gif/custom_behavior.gif 183 | :alt: Select Parameters 184 | :width: 400px 185 | :align: center 186 | ``` -------------------------------------------------------------------------------- /doc/source/usage/configure_dynamic_random_env.md: -------------------------------------------------------------------------------- 1 | Configure dynamic random environment 2 | ==================================== 3 | 4 | The dynamic, random, and clutter environment is useful to test the navigation and collision avoidance algorithms. In this environment, the distribution, goal position, and geometry of the obstacles are random at each episode. 5 | 6 | 7 | ## Random Obstacles Configuration Parameters 8 | 9 | Python script: 10 | 11 | ```python 12 | import irsim 13 | 14 | env = irsim.make() 15 | 16 | for i in range(1000): 17 | 18 | env.step() 19 | env.render(0.05) 20 | 21 | if env.done(): 22 | break 23 | 24 | env.end() 25 | ``` 26 | 27 | YAML File: 28 | 29 | ```yaml 30 | world: 31 | height: 50 # the height of the world 32 | width: 50 # the width of the world 33 | control_mode: 'keyboard' 34 | 35 | robot: 36 | - kinematics: {name: 'acker'} 37 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 38 | state: [5, 5, 0, 0] 39 | goal: [40, 40, 0] 40 | vel_max: [4, 1] 41 | 42 | sensors: 43 | - name: 'lidar2d' 44 | range_min: 0 45 | range_max: 20 46 | angle_range: 3.14 47 | number: 100 48 | 49 | plot: 50 | show_trajectory: True 51 | 52 | obstacle: 53 | 54 | - number: 20 55 | kinematics: {name: 'omni'} 56 | distribution: {name: 'random', range_low: [10, 10, -3.14], range_high: [40, 40, 3.14]} 57 | behavior: {name: 'rvo', wander: True, range_low: [10, 10, -3.14], range_high: [40, 40, 3.14], vxmax: 2, vymax: 2, factor: 3.0} 58 | vel_max: [4, 4] 59 | vel_min: [-4, -4] 60 | shape: 61 | - {name: 'circle', radius: 1.0, random_shape: True} 62 | - {name: 'polygon', random_shape: true, avg_radius_range: [0.5, 2.0], irregularity_range: [0, 0.4], spikeyness_range: [0, 0.4], num_vertices_range: [4, 6]} 63 | ``` 64 | 65 | The demonstration is shown in the following figure: 66 | 67 | ```{image} gif/random_obstacles.gif 68 | :alt: random_obstacles 69 | :width: 400px 70 | :align: center 71 | ``` 72 | 73 | ## Important Parameters Explanation 74 | 75 | The parameters to generate random obstacles with various shapes are the settings of `behavior`, `distribution`, and `shape` in the `obstacle` section. 76 | 77 | - For the `rvo` behavior, set the `wander` to `True` to enable the random movement of the obstacles when they reach the goal position. And the `rvo` behavior is used to avoid the collision among obstacles. `range_low` and `range_high` are the lower and upper bounds of the random distribution of the goal position of the obstacles. 78 | 79 | - The `distribution` parameter with name `random` is used to set the random distribution of the obstacles in a certain area. The `range_low` and `range_high` are the lower and upper bounds of the random distribution of the initial position of the obstacles. 80 | 81 | - The `shape` parameter is used to set the random shape of the obstacles by setting the `random_shape` to `True`. 82 | - For circular obstacles, the `radius` will be randomly generated within `radius_range`. 83 | - For polygon obstacles, `avg_radius_range`, `irregularity_range`, `num_vertices_range`, and `spikeyness_range` define which type of polygon will be generated. See [random_generate_polygon](#irsim.lib.algorithm.generation.random_generate_polygon) for details. 84 | 85 | The details of the parameters are listed in the [YAML Configuration](../yaml_config/configuration/) 86 | 87 | :::{tip} 88 | To generate a convex polygon, you can set `spikeyness_range` to [0, 0] or set `is_convex` to True. 89 | ::: -------------------------------------------------------------------------------- /doc/source/usage/configure_grid_map.md: -------------------------------------------------------------------------------- 1 | Configure grid map environment 2 | ============================== 3 | 4 | The grid map environment is a 2D grid-based environment that can be used to simulate various scenarios. It can be simply configured by specifying path of image file in the YAML configuration file. 5 | 6 | 7 | ## Grid Map Configuration Parameters 8 | 9 | The python script and YAML configuration file for the grid map environment are shown below: 10 | 11 | ```python 12 | 13 | import irsim 14 | 15 | env = irsim.make() 16 | 17 | for i in range(1000): 18 | 19 | env.step() 20 | env.render(0.05) 21 | 22 | if env.done(): 23 | break 24 | 25 | env.end() 26 | ``` 27 | 28 | ```yaml 29 | world: 30 | height: 50 31 | width: 50 32 | obstacle_map: 'cave.png' 33 | mdownsample: 2 34 | 35 | robot: 36 | - kinematics: {name: 'acker'} 37 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 38 | state: [5, 5, 0, 0] 39 | goal: [40, 40, 0] 40 | vel_max: [4, 1] 41 | plot: 42 | show_trail: True 43 | traj_color: 'g' 44 | show_trajectory: True 45 | show_goal: False 46 | 47 | sensors: 48 | - name: 'lidar2d' 49 | range_min: 0 50 | range_max: 20 51 | angle_range: 3.14 52 | number: 100 53 | alpha: 0.4 54 | 55 | 56 | obstacle: 57 | - number: 10 58 | distribution: {name: 'manual'} 59 | shape: 60 | - {name: 'polygon', random_shape: true, center_range: [5, 10, 40, 30], avg_radius_range: [0.5, 2]} 61 | 62 | ``` 63 | 64 | The demonstration is shown below: 65 | 66 | ```{image} gif/grid_map.gif 67 | :alt: Select Parameters 68 | :width: 400px 69 | :align: center 70 | ``` 71 | 72 | ### Important Parameters Explained 73 | 74 | To configure the grid map environment, the `obstacle_map` in the `world` section should be specified. The `mdownsample` parameter is used to downsample the image for acceleration. The image of `cave.png` should be placed in the same directory as the python script, and is shown below: 75 | 76 | ```{image} ../cave.png 77 | :alt: Select Parameters 78 | :width: 400px 79 | :align: center 80 | ``` 81 | 82 | In the simulation, this png figure will be rasterized into a grid map. Black pixels represent obstacles, and white pixels represent free space. 83 | 84 | :::{tip} 85 | You can use custom png images to create different grid map environments. The absolute or relative paths can be used to specify the image file in other directories. 86 | ::: -------------------------------------------------------------------------------- /doc/source/usage/configure_keyboard_control.md: -------------------------------------------------------------------------------- 1 | Configure keyboard control 2 | ========================== 3 | 4 | The keyboard control is a manual control method that allows you to control the robot using the keyboard. In this mode, the behavior of the robot is controlled by the user and the settings in the YAML file are ignored. 5 | 6 | ## Keyboard Control Configuration Parameters 7 | 8 | Please make sure the dependencies are installed before running the simulation with keyboard. The dependencies can be installed using the following command: 9 | 10 | ```bash 11 | pip install ir-sim[keyboard] 12 | ``` 13 | 14 | To start with the keyboard control, you can simply to specify the `control_mode` parameter in the `world` section as `keyboard`. The example of the keyboard control is shown below: 15 | 16 | ```python 17 | import irsim 18 | 19 | env = irsim.make() 20 | 21 | for i in range(1000): 22 | 23 | env.step() 24 | env.render(0.05) 25 | 26 | if env.done(): 27 | break 28 | 29 | env.end() 30 | ``` 31 | 32 | ```yaml 33 | world: 34 | height: 50 35 | width: 50 36 | control_mode: 'keyboard' 37 | obstacle_map: 'cave.png' 38 | mdownsample: 2 39 | 40 | robot: 41 | - kinematics: {name: 'acker'} 42 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 43 | state: [5, 5, 0, 0] 44 | goal: [40, 40, 0] 45 | vel_max: [4, 1] 46 | plot: 47 | show_trail: True 48 | traj_color: 'g' 49 | show_trajectory: True 50 | show_goal: False 51 | 52 | sensors: 53 | - type: 'lidar2d' 54 | range_min: 0 55 | range_max: 20 56 | angle_range: 3.14 57 | number: 100 58 | alpha: 0.4 59 | ``` 60 | 61 | The demonstration controlled by the keyboard is shown below: 62 | 63 | ```{image} gif/keyboard.gif 64 | :alt: Select Parameters 65 | :width: 400px 66 | :align: center 67 | ``` 68 | 69 | ## Keyboard Control Key Mapping 70 | 71 | | Key | Function | 72 | |----------|---------------------------------| 73 | | `w` | Forward | 74 | | `s` | Back Forward | 75 | | `a` | Turn Left | 76 | | `d` | Turn Right | 77 | | `q` | Decrease Linear Velocity | 78 | | `e` | Increase Linear Velocity | 79 | | `z` | Decrease Angular Velocity | 80 | | `c` | Increase Angular Velocity | 81 | | `alt+num`| Change Current Control Robot ID | 82 | | `r` | Reset the Environment | 83 | -------------------------------------------------------------------------------- /doc/source/usage/configure_sensor.md: -------------------------------------------------------------------------------- 1 | Configure Sensors for the robot 2 | ================================= 3 | 4 | ## LiDAR Configuration Parameters 5 | 6 | The YAML configuration file and Python Script below shows an example of a robot with a 2D LiDAR sensor: 7 | 8 | Python script: 9 | 10 | ```python 11 | import irsim 12 | 13 | env = irsim.make() 14 | 15 | for i in range(1000): 16 | 17 | env.step() 18 | env.render(0.05) 19 | 20 | if env.done(): 21 | break 22 | 23 | env.end() 24 | ``` 25 | 26 | YAML file (same name as the python script): 27 | 28 | ```yaml 29 | world: 30 | height: 10 31 | width: 10 32 | 33 | robot: 34 | - kinematics: {name: 'diff'} # omni, diff, acker 35 | shape: {name: 'circle', radius: 0.2} # radius 36 | goal: [9, 9, 0] 37 | 38 | sensors: 39 | - name: 'lidar2d' 40 | range_min: 0 41 | range_max: 5 42 | angle_range: 3.14 43 | number: 200 44 | noise: False 45 | alpha: 0.3 46 | 47 | obstacle: 48 | - shape: {name: 'circle', radius: 1.0} # radius 49 | state: [5, 5, 0] 50 | 51 | - shape: {name: 'rectangle', length: 1.5, width: 1.2} # length, width 52 | state: [6, 5, 1] 53 | 54 | - shape: {name: 'linestring', vertices: [[10, 5], [4, 0], [6, 7]]} # vertices 55 | state: [0, 0, 0] 56 | ``` 57 | 58 | 59 | 60 | The demonstration shows below: 61 | 62 | ```{image} gif/lidar2d.gif 63 | :alt: Select Parameters 64 | :width: 400px 65 | :align: center 66 | ``` 67 | 68 | ### Important Parameters Explained 69 | 70 | To configure the 2D LiDAR sensor, the sensor name of `lidar2d` should be defined in the `sensors` section of the robot. Key parameters of the LiDAR sensor are explained below: 71 | 72 | - **range_min**: The minimum range of the laser beam. 73 | - **range_max**: The maximum range of the laser beam. 74 | - **angle_range**: The angle range of the laser beam. 75 | - **number**: The number of beams. 76 | - **alpha**: The transparency of the laser beam. 77 | 78 | A full list of parameters can be found in the [YAML Configuration](#../yaml_config/configuration/). 79 | 80 | 81 | ### Advanced Lidar Configuration with noise 82 | 83 | To add noise to the LiDAR sensor, you can set the `noise` parameter to `True`. The YAML configuration file: 84 | 85 | ```yaml 86 | world: 87 | height: 10 88 | width: 10 89 | 90 | robot: 91 | - kinematics: {name: 'diff'} # omni, diff, acker 92 | shape: {name: 'circle', radius: 0.2} # radius 93 | goal: [9, 9, 0] 94 | 95 | sensors: 96 | - name: 'lidar2d' 97 | range_min: 0 98 | range_max: 5 99 | angle_range: 3.14 # 4.7123 100 | number: 200 101 | noise: True 102 | std: 0.1 103 | angle_std: 0.2 104 | offset: [0, 0, 0] 105 | alpha: 0.3 106 | 107 | obstacle: 108 | - shape: {name: 'circle', radius: 1.0} # radius 109 | state: [5, 5, 0] 110 | 111 | - shape: {name: 'rectangle', length: 1.5, width: 1.2} # length, width 112 | state: [6, 5, 1] 113 | 114 | - shape: {name: 'linestring', vertices: [[10, 5], [4, 0], [6, 7]]} # vertices 115 | state: [0, 0, 0] 116 | 117 | ``` 118 | 119 | Gaussian noise is added to the LiDAR sensor with the `std` and `angle_std` parameters. The `std` parameter is the standard deviation of the range noise, and the `angle_std` parameter is the standard deviation of the angle noise. 120 | 121 | 122 | ## FOV Configuration Parameters 123 | 124 | The YAML configuration file and Python Script below shows an example of objects within the field of view (FOV). The FOV is defined by the `fov` (float) and `fov_radius` (float) parameters in the object configuration. Each object has a FOV that can detect the robot within the FOV by the function `fov_detect_object()`. 125 | 126 | Python script: 127 | 128 | ```python 129 | import irsim 130 | 131 | env = irsim.make() 132 | 133 | for i in range(200): 134 | 135 | env.step() 136 | 137 | for obs in env.obstacle_list: 138 | if obs.fov_detect_object(env.robot): 139 | print(f'The robot is in the FOV of the {obs.name}. The parameters of this obstacle are: state [x, y, theta]: {obs.state.flatten()}, velocity [linear, angular]: {obs.velocity.flatten()}, fov in radian: {obs.fov}.') 140 | 141 | env.render(figure_kwargs={'dpi': 100}) 142 | 143 | env.end() 144 | ``` 145 | 146 | YAML file (same name as the python script): 147 | 148 | ```yaml 149 | world: 150 | height: 50 151 | width: 50 152 | step_time: 0.1 153 | sample_time: 0.1 154 | offset: [0, 0] 155 | collision_mode: 'stop' 156 | control_mode: 'auto' 157 | 158 | robot: 159 | - kinematics: {name: 'diff'} # omni, diff, acker 160 | shape: {name: 'circle', radius: 0.4} 161 | state: [10, 10, 0, 0] 162 | goal: [45, 45, 0] 163 | goal_threshold: 0.4 164 | vel_max: [3, 1] 165 | vel_min: [-3, -1] 166 | behavior: {name: 'dash', wander: True, range_low: [15, 15, -3.14], range_high: [35, 35, 3.14]} 167 | plot: 168 | show_goal: True 169 | show_trajectory: True 170 | 171 | obstacle: 172 | - number: 10 173 | distribution: {name: 'random', range_low: [10, 10, -3.14], range_high: [40, 40, 3.14]} 174 | kinematics: {name: 'diff'} 175 | behavior: {name: 'rvo', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 2.0, mode: 'vo', wander: True, range_low: [15, 15, -3.14], range_high: [35, 35, 3.14], target_roles: 'all'} 176 | vel_max: [3, 3.14] 177 | vel_min: [-3, -3.14] 178 | shape: 179 | - {name: 'circle', radius: 0.5} # radius 180 | fov: 1.57 181 | fov_radius: 5.0 182 | plot: 183 | show_fov: True 184 | show_arrow: True 185 | arrow_length: 0.8 186 | ``` 187 | 188 | The demonstration shows below: 189 | 190 | ```{image} gif/fov.gif 191 | :alt: Select Parameters 192 | :width: 400px 193 | :align: center 194 | ``` 195 | 196 | -------------------------------------------------------------------------------- /doc/source/usage/gif/3d_plot.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/3d_plot.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/custom_behavior.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/custom_behavior.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/fov.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/fov.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/grid_map.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/grid_map.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/keyboard.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/keyboard.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/lidar2d.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/lidar2d.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/multi_objects.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/multi_objects.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/noise.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/noise.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/random_obstacles.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/random_obstacles.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/robot_obstacle.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/robot_obstacle.gif -------------------------------------------------------------------------------- /doc/source/usage/gif/rvo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/doc/source/usage/gif/rvo.gif -------------------------------------------------------------------------------- /doc/source/usage/index.rst: -------------------------------------------------------------------------------- 1 | User Guide 2 | =============== 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | make_environment 8 | configure_robots_obstacles 9 | configure_sensor 10 | configure_behavior 11 | configure_grid_map 12 | configure_keyboard_control 13 | configure_dynamic_random_env 14 | save_animation 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /doc/source/usage/make_environment.md: -------------------------------------------------------------------------------- 1 | Make Environment 2 | ======= 3 | 4 | ## Python script and YAML configuration file 5 | 6 | To start the simulation, you need to create an environment. The environment is a container for all the objects in the simulation. It is also responsible for updating the state of the simulation at each time step. 7 | 8 | Follow the steps below to create an environment: 9 | 10 | ```python 11 | import irsim 12 | 13 | env = irsim.make('empty_world.yaml') 14 | ``` 15 | 16 | The `make` function creates an environment from a configuration file. Support parameters can be found in [EnvBase](#irsim.env.env_base.EnvBase) class. The configuration file is a YAML file that specifies the properties of the environment. The `empty_world.yaml` file is a simple configuration file that creates an empty environment. This file is listed below: 17 | 18 | ```yaml 19 | world: 20 | height: 10 # the height of the world 21 | width: 10 # the width of the world 22 | step_time: 0.1 # 10Hz calculate each step 23 | sample_time: 0.1 # 10 Hz for render and data extraction 24 | offset: [0, 0] # the offset of the world on x and y 25 | control_mode: 'auto' # auto, keyboard 26 | collision_mode: 'stop' # stop, reactive, unobstructed, unobstructed_obstacles 27 | obstacle_map: null # the path of obstacle map 28 | ``` 29 | 30 | ## Important Parameters Explanation 31 | 32 | - The `world` section specifies the properties of the world. 33 | - The `height` and `width` parameters specify the size of the world. 34 | - The `step_time` parameter specifies the time step for the simulation. 35 | - The `sample_time` parameter specifies the time step for rendering and data extraction. 36 | - The `offset` parameter specifies the offset of the world on the x and y axes. 37 | - The `control_mode` parameter specifies the control mode of the simulation. 38 | - The `collision_mode` parameter specifies the collision mode of the simulation. 39 | - The `obstacle_map` parameter specifies the path of the obstacle map. 40 | 41 | Details of the parameters can be found in the [YAML Configuration](#../yaml_config/configuration/). 42 | 43 | :::{tip} 44 | The default YAML configuration file is same as the name of python script. Thus, if you create a python script named `test.py`, the default YAML configuration file is `test.yaml`. And you can simply use `irsim.make()` to create the environment. Please place the YAML configuration file in the same directory as the python script. 45 | 46 | ```python 47 | import irsim 48 | 49 | env = irsim.make() 50 | ``` 51 | ::: 52 | 53 | -------------------------------------------------------------------------------- /doc/source/usage/save_animation.md: -------------------------------------------------------------------------------- 1 | Render and Save Animation 2 | ============== 3 | 4 | ## Render the environment 5 | 6 | You can render the environment by calling the [env.render()](#irsim.env.env_base.EnvBase.render) function and change the arguments of this function to control the rendering of each frame. The main parameters are: 7 | 8 | - **`interval`:** The time interval between frames. Default 0.05. This parameter is used to control the speed of visualization. 9 | - **`figure_kwargs`:** The parameters of the figures. Such as the transparent, bbox_inches, dpi, etc. Default {}. See [savefig](https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.savefig.html) for more details. 10 | - **`kwargs`:** These additional parameters are passed to the all the [object.plot](#irsim.world.object_base.ObjectBase.plot) function for default settings. 11 | 12 | ## Save the animation as GIF file 13 | 14 | You can save the animation of the simulation as a gif file very easily by setting the `save_ani` to `True` in the `make()` function: 15 | 16 | ```python 17 | 18 | env = irsim.make(save_ani=True) 19 | 20 | for i in range(300): 21 | 22 | env.step() 23 | env.render(0.05) 24 | 25 | if env.done(): 26 | break 27 | 28 | env.end(ending_time=3) 29 | ``` 30 | 31 | `ending_time` denotes how long the figure will be closed. The animation generation is also performed in this `env.end()` function. You can set the additional arguments in this function to control the animation generation. Details of the parameters can be found in the [env.end()](#irsim.env.env_base.EnvBase.end). 32 | 33 | Some common parameters for `GIF` format you may use are: 34 | 35 | - **`loop`:** The number of times the GIF should loop. Default 0 (meaning loop indefinitely). 36 | - **`duration `:** The duration (in seconds) of each frame. Either specify one value that is used for all frames, or one value for each frame. 37 | - **`fps`:** The number of frames per second. If duration is not given, the duration for each frame is set to 1/fps. Default 10. 38 | - **`subrectangles `:** If True, will try and optimize the GIF by storing only the rectangular parts of each frame that change with respect to the previous. Default True. 39 | 40 | :::{tip} 41 | The principle of the animation generation is to save the images of each frame and then combine them into a gif file. 42 | ::: 43 | 44 | ## Save the animation as a video 45 | 46 | You can save the animation of the simulation as a video file such as mp4 file by setting the suffix of the file to `.mp4` in the [env.end()](#irsim.env.env_base.EnvBase.end) function. Please make sure you have the `ffmpeg` installed in your system by `pip install imageio[ffmpeg]`. The example is shown below: 47 | 48 | ```python 49 | 50 | env = irsim.make(save_ani=True) 51 | 52 | for i in range(300): 53 | 54 | env.step() 55 | env.render(0.05) 56 | 57 | if env.done(): 58 | break 59 | 60 | env.end(ending_time=3, suffix='.mp4') 61 | ``` 62 | 63 | :::{tip} 64 | More `suffix` for the animation file format can be found on [imageio docs](https://imageio.readthedocs.io/en/stable/formats/video_formats.html) 65 | ::: 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | ## 3D Plot 74 | 75 | You can simply set the `projection` parameter to `3d` in `irsim.make` function to render the 3D plot of the simulation. The example is shown below: 76 | 77 | ```python 78 | 79 | env = irsim.make(projection='3d') 80 | 81 | for i in range(300): 82 | 83 | env.step() 84 | env.render(0.05) 85 | 86 | if env.done(): 87 | break 88 | 89 | env.end(3) 90 | ``` 91 | 92 | ```{image} gif/3d_plot.gif 93 | :alt: Select Parameters 94 | :width: 400px 95 | :align: center 96 | ``` 97 | 98 | :::{note} 99 | Currently, the 3D plot only visualizes the 2D objects in the 3D space. The 3D objects are not supported yet. 100 | ::: 101 | 102 | 103 | -------------------------------------------------------------------------------- /doc/source/yaml_config/index.rst: -------------------------------------------------------------------------------- 1 | YAML Configuration Syntax 2 | =============== 3 | 4 | .. toctree:: 5 | :maxdepth: 2 6 | 7 | configuration 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /irsim/__init__.py: -------------------------------------------------------------------------------- 1 | from .version import __version__ 2 | from irsim.env import EnvBase, EnvBase3D 3 | import os 4 | import sys 5 | 6 | 7 | def make(world_name=None, projection=None, **kwargs) -> EnvBase: 8 | 9 | ''' 10 | Create an environment by the given world file and projection. 11 | 12 | Args: 13 | world_name (str): The name of the world file. If not specified, the default name of the `python script` will be used. 14 | projection (str): The projection of the environment. Default is "None". If set to "3d", the environment will be a 3D plot environment. 15 | **kwargs: Additional keyword arguments for 16 | :py:class:`.EnvBase` or :py:class:`.EnvBase3D` for more details. 17 | Returns: 18 | The environment object 19 | ''' 20 | 21 | world_name = world_name or os.path.basename(sys.argv[0]).split(".")[0] + ".yaml" 22 | 23 | if projection == "3d": 24 | return EnvBase3D(world_name, **kwargs) 25 | else: 26 | return EnvBase(world_name, **kwargs) 27 | -------------------------------------------------------------------------------- /irsim/env/__init__.py: -------------------------------------------------------------------------------- 1 | from irsim.env.env_base import EnvBase 2 | from irsim.env.env_base3d import EnvBase3D 3 | 4 | -------------------------------------------------------------------------------- /irsim/env/env_base3d.py: -------------------------------------------------------------------------------- 1 | from irsim.env import EnvBase 2 | from .env_plot3d import EnvPlot3D 3 | from irsim.world.object_factory import ObjectFactory 4 | from irsim.world.world3d import World3D 5 | from irsim.global_param import world_param, env_param 6 | from irsim.world.object_base import ObjectBase 7 | import itertools 8 | 9 | 10 | class EnvBase3D(EnvBase): 11 | """ 12 | This class is the 3D version of the environment class. It inherits from the :py:class:`.EnvBase` class to provide the 3D plot environment. 13 | """ 14 | 15 | def __init__(self, world_name, **kwargs): 16 | 17 | super().__init__(world_name, **kwargs) 18 | 19 | object_factory = ObjectFactory() 20 | 21 | self._world = World3D(world_name, **self.env_config.parse["world"]) 22 | 23 | ObjectBase.id_iter = itertools.count() 24 | 25 | self._robot_collection = object_factory.create_from_parse( 26 | self.env_config.parse["robot"], "robot" 27 | ) 28 | self._obstacle_collection = object_factory.create_from_parse( 29 | self.env_config.parse["obstacle"], "obstacle" 30 | ) 31 | self._map_collection = object_factory.create_from_map( 32 | self._world.obstacle_positions, self._world.buffer_reso 33 | ) 34 | 35 | self._env_plot.close() 36 | 37 | self._env_plot = EnvPlot3D( 38 | self._world.grid_map, 39 | self.objects, 40 | self._world.x_range, 41 | self._world.y_range, 42 | self._world.z_range, 43 | **self.env_config.parse["plot"], 44 | ) 45 | 46 | env_param.objects = self.objects 47 | -------------------------------------------------------------------------------- /irsim/env/env_config.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | from irsim.util.util import file_check 3 | from irsim.global_param import env_param 4 | 5 | 6 | class EnvConfig: 7 | """ 8 | The base class of environment parameters read from yaml file. 9 | basic categories: world, plot, robot, obstacle 10 | See for detail 11 | """ 12 | 13 | def __init__(self, world_name) -> None: 14 | 15 | world_file_path = file_check(world_name) 16 | 17 | self._kwargs_parse = { 18 | "world": dict(), 19 | "plot": dict(), 20 | "keyboard": dict(), 21 | "robot": None, 22 | "obstacle": None, 23 | } 24 | 25 | if world_file_path != None: 26 | 27 | with open(world_file_path) as file: 28 | com_list = yaml.load(file, Loader=yaml.FullLoader) 29 | 30 | for key in com_list.keys(): 31 | if key in self._kwargs_parse.keys(): 32 | self._kwargs_parse[key] = com_list[key] 33 | else: 34 | self.logger.error( 35 | f"There are invalid key: '{key}' in {world_name} file!" 36 | ) 37 | raise KeyError 38 | 39 | else: 40 | self.logger.warning(f"{world_name} YAML File not found!, using default world config.") 41 | 42 | @property 43 | def parse(self): 44 | """ 45 | The parsed kwargs from the yaml file. 46 | """ 47 | return self._kwargs_parse 48 | 49 | @property 50 | def logger(self): 51 | ''' 52 | Get the logger of the env_param. 53 | ''' 54 | return env_param.logger 55 | -------------------------------------------------------------------------------- /irsim/env/env_logger.py: -------------------------------------------------------------------------------- 1 | from loguru import logger 2 | import sys 3 | from typing import Optional 4 | 5 | class EnvLogger: 6 | def __init__(self, log_file: Optional['str']="irsim_error.log", log_level="WARNING"): 7 | """ 8 | Initialize the EnvLogger. 9 | 10 | Args: 11 | ---- 12 | log_file (str, optional): Path to the log file. Default is 'irsim_error.log'. 13 | log_level (str, optional): Logging level. Default is 'WARNING'. 14 | """ 15 | logger.remove() 16 | logger.add( 17 | sys.stdout, 18 | level=log_level, 19 | format="{time:YYYY-MM-DD HH:mm} | " 20 | "{level: <8} | " 21 | "{message}", 22 | ) 23 | 24 | if log_file is not None: 25 | logger.add(log_file, level=log_level) 26 | 27 | def info(self, msg): 28 | """ 29 | Log an info message. 30 | 31 | Args: 32 | msg (str): The message to log. 33 | """ 34 | logger.info(msg) 35 | 36 | def error(self, msg): 37 | """ 38 | Log an error message. 39 | 40 | Args: 41 | msg (str): The message to log. 42 | """ 43 | logger.error(msg) 44 | 45 | def debug(self, msg): 46 | """ 47 | Log a debug message. 48 | 49 | Args: 50 | msg (str): The message to log. 51 | """ 52 | logger.debug(msg) 53 | 54 | def warning(self, msg): 55 | """ 56 | Log a warning message. 57 | 58 | Args: 59 | msg (str): The message to log. 60 | """ 61 | logger.warning(msg) 62 | -------------------------------------------------------------------------------- /irsim/env/env_plot3d.py: -------------------------------------------------------------------------------- 1 | from .env_plot import EnvPlot 2 | import mpl_toolkits.mplot3d.art3d as art3d 3 | from mpl_toolkits.mplot3d import Axes3D 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | from math import cos, sin 7 | 8 | 9 | class EnvPlot3D(EnvPlot): 10 | 11 | def __init__( 12 | self, 13 | grid_map=None, 14 | objects=[], 15 | x_range=[0, 10], 16 | y_range=[0, 10], 17 | z_range=[0, 10], 18 | saved_figure=dict(), 19 | figure_pixels: list = [1920, 1080], 20 | **kwargs, 21 | ): 22 | super().__init__( 23 | grid_map, 24 | objects, 25 | x_range, 26 | y_range, 27 | saved_figure, 28 | figure_pixels, 29 | **kwargs, 30 | ) 31 | 32 | # self.clear_components() 33 | # self.ax.remove() 34 | 35 | self.ax = self.fig.add_subplot(projection="3d") 36 | self.z_range = z_range 37 | 38 | self.init_plot(grid_map, objects, **kwargs) 39 | self.ax.set_zlim(z_range) 40 | 41 | def draw_points(self, points, s=10, c="m", refresh=True, **kwargs): 42 | """ 43 | Draw points on the plot. 44 | 45 | Args: 46 | points (list): List of points, each point as [x, y, z]. 47 | s (int): Size of the points. 48 | c (str): Color of the points. 49 | refresh (bool): Whether to refresh the plot. 50 | kwargs: Additional plotting options. 51 | """ 52 | 53 | if points is None: 54 | return 55 | 56 | if isinstance(points, list): 57 | x_coordinates = [point[0] for point in points] 58 | y_coordinates = [point[1] for point in points] 59 | z_coordinates = [point[2] for point in points] 60 | 61 | elif isinstance(points, np.ndarray): 62 | 63 | if points.shape[1] > 1: 64 | x_coordinates = [point[0] for point in points.T] 65 | y_coordinates = [point[1] for point in points.T] 66 | z_coordinates = [point[2] for point in points.T] 67 | else: 68 | x_coordinates = points[0] 69 | y_coordinates = points[1] 70 | z_coordinates = points[2] 71 | 72 | points = self.ax.scatter( 73 | x_coordinates, y_coordinates, z_coordinates, "z", s, c, **kwargs 74 | ) 75 | 76 | if refresh: 77 | self.dyna_point_list.append(points) 78 | 79 | 80 | def draw_quiver(self, point, refresh=False, **kwargs): 81 | """ 82 | Draw a quiver plot on the plot. 83 | 84 | Args: 85 | points (6*1 np.ndarray): List of points, each point as [x, y, z, u, v, w]. u, v, w are the components of the vector. 86 | kwargs: Additional plotting options. 87 | """ 88 | 89 | if point is None: 90 | return 91 | 92 | ax_point = self.ax.scatter(point[0], point[1], point[2], color=kwargs.get('point_color', 'blue'), label='Points') 93 | 94 | ax_quiver = self.ax.quiver( 95 | point[0], point[1], point[2], # starting positions 96 | point[3], point[4], point[5], # vector components (direction) 97 | length=0.2, normalize=True, color=kwargs.get('quiver_color', 'red'), label='Direction' 98 | ) 99 | 100 | if refresh: 101 | self.dyna_quiver_list.append(ax_quiver) 102 | self.dyna_point_list.append(ax_point) 103 | 104 | def draw_quivers(self, points, refresh=False, **kwargs): 105 | """ 106 | Draw a series of quiver plot on the plot. 107 | 108 | Args: 109 | points (list or np.ndarray): List of points, each point as [x, y, z, u, v, w]. u, v, w are the components of the vector. 110 | 111 | """ 112 | 113 | for point in points.T: 114 | self.draw_quiver(point, refresh, **kwargs) 115 | 116 | def draw_trajectory( 117 | self, 118 | traj, 119 | traj_type="g-", 120 | label="trajectory", 121 | show_direction=False, 122 | refresh=False, 123 | **kwargs, 124 | ): 125 | """ 126 | Draw a trajectory on the plot. 127 | 128 | Args: 129 | traj (list or np.ndarray): List of points or array of points [x, y, z]. 130 | traj_type (str): Type of trajectory line (e.g., 'g-'). 131 | See https://matplotlib.org/3.1.1/api/_as_gen/matplotlib.pyplot.plot.html for details. 132 | label (str): Label for the trajectory. 133 | show_direction (bool): Whether to show the direction of the trajectory. 134 | refresh (bool): Whether to refresh the plot. 135 | kwargs: Additional plotting options for ax.plot() 136 | """ 137 | if isinstance(traj, list): 138 | path_x_list = [p[0, 0] for p in traj] 139 | path_y_list = [p[1, 0] for p in traj] 140 | path_z_list = [p[2, 0] for p in traj] 141 | elif isinstance(traj, np.ndarray): 142 | path_x_list = [p[0] for p in traj.T] 143 | path_y_list = [p[1] for p in traj.T] 144 | path_z_list = [p[2] for p in traj.T] 145 | 146 | line = self.ax.plot( 147 | path_x_list, path_y_list, path_z_list, traj_type, label=label, **kwargs 148 | ) 149 | 150 | if show_direction: 151 | print("Not support currently") 152 | # if isinstance(traj, list): 153 | # u_list = [cos(p[2, 0]) for p in traj] 154 | # v_list = [sin(p[2, 0]) for p in traj] 155 | # elif isinstance(traj, np.ndarray): 156 | # u_list = [cos(p[2]) for p in traj.T] 157 | # v_list = [sin(p[2]) for p in traj.T] 158 | 159 | # if isinstance(self.ax, Axes3D): 160 | # path_z_list = [0] * len(path_x_list) 161 | # w_list = [0] * len(u_list) 162 | 163 | # self.ax.quiver(path_x_list, path_y_list, path_z_list, u_list, v_list, w_list) 164 | 165 | # else: 166 | # self.ax.quiver(path_x_list, path_y_list, u_list, v_list) 167 | 168 | if refresh: 169 | self.dyna_line_list.append(line) 170 | -------------------------------------------------------------------------------- /irsim/global_param/env_param.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Objects: A list of all objects in the environment 3 | Logger: A logger object to log messages 4 | Platform: The operating system platform 5 | ''' 6 | 7 | import platform 8 | 9 | objects = [] 10 | logger = None 11 | GeometryTree = None 12 | platform_name = platform.system() 13 | -------------------------------------------------------------------------------- /irsim/global_param/path_param.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from dataclasses import dataclass 3 | import os 4 | import irsim 5 | 6 | @dataclass 7 | class PathManager: 8 | 9 | ''' 10 | Module for managing the path of the project. 11 | root_path: path of the irsim package 12 | ani_buffer_path: path of the animation buffer 13 | ani_path: path of the animation 14 | fig_path: path of the saved figure 15 | ''' 16 | 17 | root_path: str = os.path.dirname(irsim.__file__) 18 | ani_buffer_path: str = sys.path[0] + "/animation_buffer" 19 | ani_path: str = sys.path[0] + "/animation" 20 | fig_path: str = sys.path[0] + "/figure" 21 | 22 | path_manager = PathManager() 23 | -------------------------------------------------------------------------------- /irsim/global_param/world_param.py: -------------------------------------------------------------------------------- 1 | import os 2 | import irsim 3 | 4 | 5 | ''' 6 | world parameters: 7 | time: time elapse of the simulation 8 | control_mode: 9 | - auto: robot will be controlled automatically 10 | - keyboard: robot will be controlled by keyboard 11 | 12 | collision_mode: 13 | - stop (default): All Objects stop when collision, 14 | - unobstructed: No collision check 15 | - reactive: robot will have reaction when collision with others 16 | - unobstructed_obstacles: Only allows obstacles to pass through each other without consideration of any collision. The robots will stop when they are in collision with the obstacles. 17 | 18 | step_time: time of the simulation step, default is 0.1 19 | count: count of the simulation, time = count * step_time 20 | 21 | ''' 22 | 23 | time = 0 24 | control_mode = "auto" 25 | collision_mode = "stop" 26 | 27 | step_time = 0.1 28 | count = 0 29 | -------------------------------------------------------------------------------- /irsim/lib/__init__.py: -------------------------------------------------------------------------------- 1 | from irsim.lib.algorithm.kinematics import ( 2 | differential_kinematics, 3 | ackermann_kinematics, 4 | omni_kinematics, 5 | ) 6 | 7 | kinematics_factory = { 8 | "diff": differential_kinematics, 9 | "acker": ackermann_kinematics, 10 | "omni": omni_kinematics, 11 | } 12 | 13 | from irsim.lib.behavior.behavior_registry import register_behavior 14 | from irsim.lib.behavior.behavior import Behavior 15 | 16 | from irsim.lib.algorithm.generation import random_generate_polygon, generate_polygon 17 | from irsim.lib.algorithm.rvo import reciprocal_vel_obs 18 | 19 | from irsim.lib.handler.kinematics_handler import KinematicsFactory 20 | from irsim.lib.handler.geometry_handler import GeometryFactory 21 | 22 | -------------------------------------------------------------------------------- /irsim/lib/algorithm/generation.py: -------------------------------------------------------------------------------- 1 | 2 | ''' 3 | This file is the implementation of the generation of random polygons. 4 | 5 | Author: Ruihua Han 6 | ''' 7 | 8 | import math 9 | import random 10 | import numpy as np 11 | from typing import List 12 | 13 | def random_generate_polygon( 14 | number=1, 15 | center_range=[0, 0, 0, 0], 16 | avg_radius_range=[0.1, 1], 17 | irregularity_range=[0, 1], 18 | spikeyness_range=[0, 1], 19 | num_vertices_range=[4, 10], 20 | **kwargs, 21 | ): 22 | """ 23 | reference: https://stackoverflow.com/questions/8997099/algorithm-to-generate-random-2d-polygon 24 | 25 | Generate random polygons with specified properties. 26 | 27 | Args: 28 | number (int): Number of polygons to generate (default 1). 29 | center_range (List[float]): Range for the polygon center [min_x, min_y, max_x, max_y]. 30 | avg_radius_range (List[float]): Range for the average radius of the polygons. 31 | irregularity_range (List[float]): Range for the irregularity of the polygons. 32 | spikeyness_range (List[float]): Range for the spikeyness of the polygons. 33 | num_vertices_range (List[int]): Range for the number of vertices of the polygons. 34 | 35 | Returns: 36 | List of vertices for each polygon or a single polygon's vertices if number=1. 37 | """ 38 | center = np.random.uniform( 39 | low=center_range[0:2], high=center_range[2:], size=(number, 2) 40 | ) 41 | avg_radius = np.random.uniform( 42 | low=avg_radius_range[0], high=avg_radius_range[1], size=(number,) 43 | ) 44 | irregularity = np.random.uniform( 45 | low=irregularity_range[0], high=irregularity_range[1], size=(number,) 46 | ) 47 | spikeyness = np.random.uniform( 48 | low=spikeyness_range[0], high=spikeyness_range[1], size=(number,) 49 | ) 50 | num_vertices = np.random.randint( 51 | low=num_vertices_range[0], high=num_vertices_range[1], size=(number,) 52 | ) 53 | 54 | vertices_list = [ 55 | generate_polygon( 56 | center[i, :], avg_radius[i], irregularity[i], spikeyness[i], num_vertices[i] 57 | ) 58 | for i in range(number) 59 | ] 60 | 61 | if number == 1: 62 | return vertices_list[0] 63 | 64 | return vertices_list 65 | 66 | 67 | def generate_polygon(center, avg_radius, irregularity, spikeyness, num_vertices): 68 | """ 69 | Generate a random polygon around a center point. 70 | 71 | Args: 72 | center (Tuple[float, float]): Center of the polygon. 73 | avg_radius (float): Average radius from the center to vertices. 74 | irregularity (float): Variance of angle spacing between vertices. Range [0, 1] 75 | spikeyness (float): Variance of radius from the center. Range [0, 1] 76 | num_vertices (int): Number of vertices for the polygon. 77 | 78 | Returns: 79 | numpy.ndarray: Vertices of the polygon in CCW order. 80 | """ 81 | if irregularity < 0 or irregularity > 1: 82 | raise ValueError("Irregularity must be between 0 and 1.") 83 | if spikeyness < 0 or spikeyness > 1: 84 | raise ValueError("Spikeyness must be between 0 and 1.") 85 | 86 | irregularity *= 2 * math.pi / num_vertices 87 | spikeyness *= avg_radius 88 | angle_steps = random_angle_steps(num_vertices, irregularity) 89 | 90 | points = [] 91 | angle = np.random.uniform(0, 2 * math.pi) 92 | for i in range(num_vertices): 93 | radius = clip(random.gauss(avg_radius, spikeyness), 0, 2 * avg_radius) 94 | point = ( 95 | center[0] + radius * math.cos(angle), 96 | center[1] + radius * math.sin(angle), 97 | ) 98 | points.append(point) 99 | angle += angle_steps[i] 100 | 101 | vertices = np.array(points) 102 | 103 | return vertices 104 | 105 | 106 | def random_angle_steps(steps: int, irregularity: float) -> List[float]: 107 | """ 108 | Generate random angle steps for polygon vertices. 109 | 110 | Args: 111 | steps (int): Number of angles to generate. 112 | irregularity (float): Variance of angle spacing. 113 | 114 | Returns: 115 | List[float]: Random angles in radians. 116 | """ 117 | angles = [] 118 | lower = (2 * math.pi / steps) - irregularity 119 | upper = (2 * math.pi / steps) + irregularity 120 | cumsum = 0 121 | for i in range(steps): 122 | angle = np.random.uniform(lower, upper) 123 | angles.append(angle) 124 | cumsum += angle 125 | 126 | cumsum /= 2 * math.pi 127 | for i in range(steps): 128 | angles[i] /= cumsum 129 | return angles 130 | 131 | 132 | def clip(value, lower, upper): 133 | """ 134 | Clip a value to a specified range. 135 | 136 | Args: 137 | value (float): Value to be clipped. 138 | lower (float): Lower bound of the range. 139 | upper (float): Upper bound of the range. 140 | 141 | Returns: 142 | float: Clipped value. 143 | """ 144 | return min(upper, max(value, lower)) 145 | -------------------------------------------------------------------------------- /irsim/lib/algorithm/kinematics.py: -------------------------------------------------------------------------------- 1 | 2 | ''' 3 | This file is the implementation of the kinematics for different robots. 4 | 5 | reference: Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. 1st ed. Cambridge, MA: Cambridge University Press, 2017. 6 | ''' 7 | 8 | import numpy as np 9 | from math import cos, sin, tan 10 | from irsim.util.util import WrapToPi 11 | 12 | def differential_kinematics( 13 | state, velocity, step_time, noise=False, alpha=[0.03, 0, 0, 0.03] 14 | ): 15 | """ 16 | Calculate the next state for a differential wheel robot. 17 | 18 | Args: 19 | state: A 3x1 vector [x, y, theta] representing the current position and orientation. 20 | velocity: A 2x1 vector [linear, angular] representing the current velocities. 21 | step_time: The time step for the simulation. 22 | noise: Boolean indicating whether to add noise to the velocity (default False). 23 | alpha: List of noise parameters for the velocity model (default [0.03, 0, 0, 0.03]). alpha[0] and alpha[1] are for linear velocity, alpha[2] and alpha[3] are for angular velocity. 24 | 25 | Returns: 26 | next_state: A 3x1 vector [x, y, theta] representing the next state. 27 | """ 28 | assert state.shape[0] >= 3 and velocity.shape[0] >= 2 29 | 30 | if noise: 31 | assert len(alpha) >= 4 32 | std_linear = np.sqrt( 33 | alpha[0] * (velocity[0, 0] ** 2) + alpha[1] * (velocity[1, 0] ** 2) 34 | ) 35 | std_angular = np.sqrt( 36 | alpha[2] * (velocity[0, 0] ** 2) + alpha[3] * (velocity[1, 0] ** 2) 37 | ) 38 | real_velocity = velocity + np.random.normal( 39 | [[0], [0]], scale=[[std_linear], [std_angular]] 40 | ) 41 | else: 42 | real_velocity = velocity 43 | 44 | phi = state[2, 0] 45 | co_matrix = np.array([[cos(phi), 0], [sin(phi), 0], [0, 1]]) 46 | next_state = state[0:3] + co_matrix @ real_velocity * step_time 47 | next_state[2, 0] = WrapToPi(next_state[2, 0]) 48 | 49 | return next_state 50 | 51 | 52 | def ackermann_kinematics( 53 | state, 54 | velocity, 55 | step_time, 56 | noise=False, 57 | alpha=[0.03, 0, 0, 0.03], 58 | mode="steer", 59 | wheelbase=1, 60 | ): 61 | """ 62 | Calculate the next state for an Ackermann steering vehicle. 63 | 64 | Args: 65 | state: A 4x1 vector [x, y, theta, steer_angle] representing the current state. 66 | velocity: A 2x1 vector representing the current velocities, format depends on mode. 67 | For "steer" mode, [linear, steer_angle] is expected. 68 | For "angular" mode, [linear, angular] is expected. 69 | 70 | step_time: The time step for the simulation. 71 | noise: Boolean indicating whether to add noise to the velocity (default False). 72 | alpha: List of noise parameters for the velocity model (default [0.03, 0, 0, 0.03]). alpha[0] and alpha[1] are for linear velocity, alpha[2] and alpha[3] are for angular velocity. 73 | mode: The kinematic mode, either "steer" or "angular" (default "steer"). 74 | wheelbase: The distance between the front and rear axles (default 1). 75 | 76 | Returns: 77 | new_state: A 4x1 vector representing the next state. 78 | """ 79 | assert state.shape[0] >= 4 and velocity.shape[0] >= 2 80 | 81 | phi = state[2, 0] 82 | psi = state[3, 0] 83 | 84 | if noise: 85 | assert len(alpha) >= 4 86 | std_linear = np.sqrt( 87 | alpha[0] * (velocity[0, 0] ** 2) + alpha[1] * (velocity[1, 0] ** 2) 88 | ) 89 | std_angular = np.sqrt( 90 | alpha[2] * (velocity[0, 0] ** 2) + alpha[3] * (velocity[1, 0] ** 2) 91 | ) 92 | real_velocity = velocity + np.random.normal( 93 | [[0], [0]], scale=[[std_linear], [std_angular]] 94 | ) 95 | else: 96 | real_velocity = velocity 97 | 98 | if mode == "steer": 99 | co_matrix = np.array( 100 | [[cos(phi), 0], [sin(phi), 0], [tan(psi) / wheelbase, 0], [0, 1]] 101 | ) 102 | elif mode == "angular": 103 | co_matrix = np.array( 104 | [[cos(phi), 0], [sin(phi), 0], [tan(psi) / wheelbase, 0], [0, 1]] 105 | ) 106 | 107 | d_state = co_matrix @ real_velocity 108 | new_state = state + d_state * step_time 109 | 110 | if mode == "steer": 111 | new_state[3, 0] = real_velocity[1, 0] 112 | 113 | new_state[2, 0] = WrapToPi(new_state[2, 0]) 114 | 115 | return new_state 116 | 117 | 118 | def omni_kinematics(state, velocity, step_time, noise=False, alpha=[0.03, 0, 0, 0.03]): 119 | """ 120 | Calculate the next position for an omnidirectional robot. 121 | 122 | Args: 123 | state: A 2x1 vector [x, y] representing the current position. 124 | velocity: A 2x1 vector [vx, vy] representing the current velocities. 125 | step_time: The time step for the simulation. 126 | noise: Boolean indicating whether to add noise to the velocity (default False). 127 | alpha: List of noise parameters for the velocity model (default [0.03, 0.03]). alpha[0] is for x velocity, alpha[1] is for y velocity. 128 | 129 | Returns: 130 | new_position: A 2x1 vector [x, y] representing the next position. 131 | """ 132 | 133 | assert velocity.shape[0] >= 2 and state.shape[0] >= 2 134 | 135 | if noise: 136 | assert len(alpha) >= 2 137 | std_vx = np.sqrt(alpha[0]) 138 | std_vy = np.sqrt(alpha[-1]) 139 | real_velocity = velocity + np.random.normal( 140 | [[0], [0]], scale=[[std_vx], [std_vy]] 141 | ) 142 | else: 143 | real_velocity = velocity 144 | 145 | new_position = state[0:2] + real_velocity * step_time 146 | 147 | return new_position 148 | 149 | -------------------------------------------------------------------------------- /irsim/lib/behavior/behavior.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from irsim.global_param import env_param, world_param 3 | import importlib 4 | from typing import Tuple, Any 5 | from .behavior_registry import behaviors_map 6 | 7 | 8 | 9 | class Behavior: 10 | """ 11 | Represents the behavior of an agent in the simulation. 12 | 13 | Args: 14 | object_info (object): Object information from the object_base class ObjectInfo. 15 | behavior_dict (dict): Dictionary containing behavior parameters for different behaviors. 16 | Name Options include: 'dash', 'rvo'. 17 | target_roles: 18 | 'all': all objects in the environment will be considered within this behavior. 19 | 'obstacle': only obstacles will be considered within this behavior. 20 | 'robot': only robots will be considered within this behavior. 21 | """ 22 | 23 | def __init__(self, object_info=None, behavior_dict=None) -> None: 24 | """ 25 | Initializes the Behavior class with object information and behavior parameters. 26 | 27 | Args: 28 | object_info (object): Information about the agent. 29 | behavior_dict (dict): Behavior parameters. 30 | """ 31 | self.object_info = object_info 32 | self.behavior_dict = dict() if behavior_dict is None else behavior_dict 33 | self.load_behavior() 34 | 35 | def gen_vel(self, ego_object, external_objects=[]): 36 | """ 37 | Generate velocity for the agent based on the behavior dictionary. 38 | 39 | Args: 40 | ego_object: the object itself 41 | external_objects: all the other objects in the environment 42 | 43 | Returns: 44 | np.array (2, 1): Generated velocity for the agent. 45 | """ 46 | 47 | if self.behavior_dict is None or not self.behavior_dict: 48 | 49 | if world_param.control_mode == "auto": 50 | if world_param.count == 1: 51 | self.logger.warning( 52 | "Behavior not defined for Object {}. This object will be static. Available behaviors: rvo, dash".format( 53 | self.object_info.id, 54 | ) 55 | ) 56 | 57 | return np.zeros((2, 1)) 58 | 59 | target_roles = self.behavior_dict.get("target_roles", "all") 60 | 61 | if target_roles == "all": 62 | external_objects = external_objects 63 | elif target_roles == "obstacle": 64 | external_objects = [ 65 | obj for obj in external_objects if obj.role == "obstacle" 66 | ] 67 | elif target_roles == "robot": 68 | external_objects = [obj for obj in external_objects if obj.role == "robot"] 69 | 70 | behavior_vel = self.invoke_behavior( 71 | self.object_info.kinematics, 72 | self.behavior_dict["name"], 73 | ego_object=ego_object, 74 | external_objects=external_objects, 75 | **self.behavior_dict, 76 | ) 77 | 78 | return behavior_vel 79 | 80 | def load_behavior(self, behaviors: str = ".behavior_methods"): 81 | """ 82 | Load behavior parameters from the script. 83 | 84 | Args: 85 | behaviors (str): name of the bevavior script. 86 | """ 87 | 88 | try: 89 | importlib.import_module(behaviors, package="irsim.lib.behavior") 90 | except ImportError as e: 91 | print(f"Failed to load module '{behaviors}': {e}") 92 | 93 | def invoke_behavior(self, kinematics: str, action: str, **kwargs: Any) -> Any: 94 | """ 95 | Invoke a behavior method. 96 | 97 | Args: 98 | kinematics (str): Name of the behavior method. only support: 'diff', 'omni', 'acker'. 99 | action: Name of the action method. example: 'dash', 'rvo'. 100 | **kwargs: Arbitrary keyword arguments. 101 | 102 | Returns: 103 | np.array: Velocity (2x1). 104 | """ 105 | key: Tuple[str, str] = (kinematics, action) 106 | func = behaviors_map.get(key) 107 | if not func: 108 | raise ValueError( 109 | f"No method found for category '{kinematics}' and action '{action}'." 110 | ) 111 | 112 | return func(**kwargs) 113 | 114 | @property 115 | def logger(self): 116 | return env_param.logger 117 | -------------------------------------------------------------------------------- /irsim/lib/behavior/behavior_registry.py: -------------------------------------------------------------------------------- 1 | from typing import Callable, Dict, Tuple, Any 2 | 3 | behaviors_map: Dict[Tuple[str, str], Callable[..., Any]] = {} 4 | 5 | 6 | def register_behavior(kinematics: str, action: str): 7 | """ 8 | decorator to register a method in the behavior registry 9 | 10 | Args: 11 | kinematics: only support diff, omni, or acker 12 | action: defined action for the kinematics 13 | """ 14 | 15 | def decorator(func: Callable[..., Any]) -> Callable[..., Any]: 16 | key = (kinematics, action) 17 | if key in behaviors_map: 18 | raise ValueError( 19 | f"Method for category '{kinematics}' and action '{action}' is already registered." 20 | ) 21 | behaviors_map[key] = func 22 | return func 23 | 24 | return decorator 25 | -------------------------------------------------------------------------------- /irsim/lib/handler/kinematics_handler.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from abc import ABC, abstractmethod 3 | from irsim.lib.algorithm.kinematics import ( 4 | differential_kinematics, 5 | ackermann_kinematics, 6 | omni_kinematics, 7 | ) 8 | 9 | 10 | class KinematicsHandler(ABC): 11 | """ 12 | Abstract base class for handling robot kinematics. 13 | """ 14 | 15 | def __init__(self, name, noise: bool = False, alpha: list = None): 16 | """ 17 | Initialize the KinematicsHandler class. 18 | 19 | Args: 20 | noise (bool): Boolean indicating whether to add noise to the velocity (default False). 21 | alpha (list): List of noise parameters for the velocity model (default [0.03, 0, 0, 0.03]). 22 | """ 23 | 24 | self.name = name 25 | self.noise = noise 26 | self.alpha = alpha or [0.03, 0, 0, 0.03] 27 | 28 | @abstractmethod 29 | def step( 30 | self, state: np.ndarray, velocity: np.ndarray, step_time: float 31 | ) -> np.ndarray: 32 | """ 33 | Calculate the next state using the kinematics model. 34 | 35 | Args: 36 | state (np.ndarray): Current state. 37 | velocity (np.ndarray): Velocity vector. 38 | step_time (float): Time step for simulation. 39 | 40 | Returns: 41 | np.ndarray: Next state. 42 | """ 43 | pass 44 | 45 | 46 | class OmniKinematics(KinematicsHandler): 47 | 48 | def __init__(self, name, noise, alpha): 49 | super().__init__(name, noise, alpha) 50 | 51 | def step( 52 | self, state: np.ndarray, velocity: np.ndarray, step_time: float 53 | ) -> np.ndarray: 54 | next_position = omni_kinematics( 55 | state[0:2], velocity, step_time, self.noise, self.alpha 56 | ) 57 | next_state = np.concatenate((next_position, state[2:])) 58 | return next_state 59 | 60 | 61 | class DifferentialKinematics(KinematicsHandler): 62 | 63 | def __init__(self, name, noise, alpha): 64 | super(DifferentialKinematics, self).__init__(name, noise, alpha) 65 | 66 | def step( 67 | self, state: np.ndarray, velocity: np.ndarray, step_time: float 68 | ) -> np.ndarray: 69 | next_state = differential_kinematics( 70 | state, velocity, step_time, self.noise, self.alpha 71 | ) 72 | return next_state 73 | 74 | 75 | class AckermannKinematics(KinematicsHandler): 76 | 77 | def __init__( 78 | self, 79 | name, 80 | noise: bool = False, 81 | alpha: list = None, 82 | mode: str = "steer", 83 | wheelbase: float = 1.0, 84 | ): 85 | super().__init__(name, noise, alpha) 86 | self.mode = mode 87 | self.wheelbase = wheelbase 88 | 89 | def step( 90 | self, state: np.ndarray, velocity: np.ndarray, step_time: float 91 | ) -> np.ndarray: 92 | next_state = ackermann_kinematics( 93 | state, 94 | velocity, 95 | step_time, 96 | self.noise, 97 | self.alpha, 98 | self.mode, 99 | self.wheelbase, 100 | ) 101 | return next_state 102 | 103 | 104 | # class Rigid3DKinematics(KinematicsHandler): 105 | 106 | # def __init__(self, name, noise, alpha): 107 | # super().__init__(name, noise, alpha) 108 | 109 | # def step(self, state: np.ndarray, velocity: np.ndarray, step_time: float) -> np.ndarray: 110 | # next_state = rigid3d_kinematics(state, velocity, step_time, self.noise, self.alpha) 111 | # return next_state 112 | 113 | 114 | class KinematicsFactory: 115 | """ 116 | Factory class to create kinematics handlers. 117 | """ 118 | 119 | @staticmethod 120 | def create_kinematics( 121 | name: str = None, 122 | noise: bool = False, 123 | alpha: list = None, 124 | mode: str = "steer", 125 | wheelbase: float = None, 126 | role: str = "robot", 127 | ) -> KinematicsHandler: 128 | name = name.lower() if name else None 129 | if name == "omni": 130 | return OmniKinematics(name, noise, alpha) 131 | elif name == "diff": 132 | return DifferentialKinematics(name, noise, alpha) 133 | elif name == "acker": 134 | return AckermannKinematics(name, noise, alpha, mode, wheelbase) 135 | # elif name == 'rigid3d': 136 | # return Rigid3DKinematics(name, noise, alpha) 137 | else: 138 | if role == "robot": 139 | print(f"Unknown kinematics type: {name}, the robot will be stationary.") 140 | else: 141 | pass 142 | 143 | return None 144 | -------------------------------------------------------------------------------- /irsim/lib/path_planners/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/lib/path_planners/__init__.py -------------------------------------------------------------------------------- /irsim/usage/01empty_world/empty_world.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make('empty_world.yaml') 4 | env.show() 5 | -------------------------------------------------------------------------------- /irsim/usage/01empty_world/empty_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | control_mode: 'auto' # auto, keyboard 8 | collision_mode: 'stop' # stop, reactive, unobstructed, unobstructed_obstacles 9 | obstacle_map: null # the obstacle map 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /irsim/usage/02robot_world/car_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | 8 | robot: 9 | - kinematics: {name: 'acker'} # omni, diff, acker 10 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 11 | state: [1, 1, 0, 0] 12 | goal: [40, 40, 0] 13 | vel_max: [4, 1] 14 | behavior: {name: 'dash'} # move toward to the goal directly 15 | plot: 16 | show_trajectory: True 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /irsim/usage/02robot_world/robot_omni_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | 8 | robot: 9 | - kinematics: {name: 'omni'} # omni, diff, acker 10 | # shape: {name: 'circle', radius: 0.2} # radius 11 | shape: {name: 'rectangle', length: 0.3, width: 1.0} 12 | state: [1, 1, 0] 13 | goal: [9, 9, 0] 14 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 15 | behavior: {name: 'dash'} # move toward to the goal directly 16 | color: 'g' 17 | state_dim: 3 18 | plot: 19 | show_trajectory: True 20 | # description: 'diff_robot0.png' 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /irsim/usage/02robot_world/robot_polygon_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | 8 | robot: 9 | kinematics: {name: 'diff'} # omni, diff, acker 10 | shape: {name: 'polygon', vertices: [[0.1, 0.2], [-0.1, 0.3], [0, -0.3], [0.3, -0.2]]} # radius 11 | behavior: {name: 'dash'} # move toward to the goal directly 12 | state: [1, 1, 0] 13 | goal: [9, 9, 0] 14 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 15 | color: 'g' 16 | plot: 17 | show_trajectory: True 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /irsim/usage/02robot_world/robot_world.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | import time 3 | env = irsim.make('robot_world.yaml') 4 | # env = irsim.make("robot_omni_world.yaml") 5 | # env = irsim.make('car_world.yaml') 6 | # env = irsim.make("robot_polygon_world.yaml") 7 | 8 | for i in range(1000): 9 | 10 | env.step() 11 | start_time = time.time() 12 | env.render() 13 | end_time = time.time() 14 | print(f"Time taken: {end_time - start_time} seconds") 15 | 16 | if env.done(): 17 | break 18 | 19 | env.end() 20 | -------------------------------------------------------------------------------- /irsim/usage/02robot_world/robot_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | plot: 8 | no_axis: true 9 | saved_figure: 10 | dpi: 200 11 | 12 | robot: 13 | kinematics: {name: 'diff'} # omni, diff, acker 14 | shape: {name: 'circle', radius: 0.2} # radius 15 | # shape: {name: 'rectangle', length: 0.3, width: 1.0} 16 | state: [1, 1, 0] 17 | goal: 18 | - [[9, 9, 0], [2, 4, 1], [3, 3, 2]] 19 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 20 | behavior: {name: dash} # move toward to the goal directly 21 | color: 'g' 22 | plot: 23 | show_trajectory: True 24 | show_goal: True 25 | # description: 'diff_robot0.png' 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /irsim/usage/03obstacle_world/obstacle_world.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make("obstacle_world.yaml") 4 | 5 | for i in range(3000): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end() 14 | -------------------------------------------------------------------------------- /irsim/usage/03obstacle_world/obstacle_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive', 'unobstructed_obstacles' 8 | 9 | robot: 10 | - kinematics: {name: 'diff'} # omni, diff, acker 11 | shape: {name: 'circle', radius: 0.2} # radius 12 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 13 | state: [1, 1, 0] 14 | goal: [9, 9, 0] 15 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 16 | behavior: {name: 'dash'} # move toward to the goal directly 17 | 18 | obstacle: 19 | - shape: {name: 'circle', radius: 1.0} # radius 20 | state: [5, 5, 0] 21 | 22 | - shape: {name: 'rectangle', length: 1.5, width: 1.2} # radius 23 | state: [6, 5, 1] 24 | 25 | - shape: {name: 'linestring', vertices: [[5, 5], [4, 0], [1, 6]] } # vertices 26 | state: [0, 0, 0] 27 | unobstructed: True 28 | 29 | - shape: 30 | name: 'polygon' 31 | vertices: 32 | - [4.5, 4.5] 33 | - [5.5, 4.5] 34 | - [5.5, 5.5] 35 | - [4.5, 5.5] 36 | -------------------------------------------------------------------------------- /irsim/usage/04collision_world/collision_world.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make('collision_world.yaml') 4 | 5 | for i in range(300): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end() 14 | -------------------------------------------------------------------------------- /irsim/usage/04collision_world/collision_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive', 'unobstructed_obstacles' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'diff'} # omni, diff, acker 12 | shape: {name: 'circle', radius: 0.2} # radius 13 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 14 | state: [1, 1, 0] 15 | goal: [9, 9, 0] 16 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 17 | behavior: {name: 'dash'} # move toward to the goal directly 18 | 19 | obstacle: 20 | - shape: {name: 'circle', radius: 1.0} # radius 21 | state: [5, 5, 0] 22 | plot: 23 | show_goal: True 24 | 25 | # - shape: {name: 'rectangle', length: 1.5, width: 1.2} # radius 26 | # state: [6, 5, 1] 27 | 28 | # - shape: {name: 'circle', radius: 0.4} # radius 29 | # state: [1, 1, 0] -------------------------------------------------------------------------------- /irsim/usage/05lidar_world/lidar_world.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make('lidar_world.yaml') 4 | # env = irsim.make('lidar_world_noise.yaml') 5 | 6 | for i in range(300): 7 | 8 | env.step() 9 | env.render(0.05) 10 | 11 | if env.done(): 12 | break 13 | 14 | env.end(3) 15 | -------------------------------------------------------------------------------- /irsim/usage/05lidar_world/lidar_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive', 'unobstructed_obstacles' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'diff'} # omni, diff, acker 12 | shape: {name: 'circle', radius: 0.2} # radius 13 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 14 | state: [1, 1, 0] 15 | goal: [9, 9, 0] 16 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 17 | behavior: {name: 'dash'} # move toward to the goal directly 18 | 19 | sensors: 20 | - name: 'lidar2d' 21 | range_min: 0 22 | range_max: 5 23 | angle_range: 3.14 # 4.7123 24 | number: 200 25 | noise: False 26 | std: 0.2 27 | angle_std: 0.2 28 | offset: [0.1, 0.1, 0.5] 29 | alpha: 0.3 30 | 31 | obstacle: 32 | - shape: {name: 'circle', radius: 1.0, center: [1, 1]} # radius 33 | state: [5, 5, 0] 34 | unobstructed: True 35 | 36 | - shape: {name: 'rectangle', length: 1.5, width: 1.2} # length, width 37 | state: [6, 5, 1] 38 | 39 | - shape: {name: 'linestring',vertices: [[10, 5], [4, 0]]} # vertices 40 | state: [0, 0, 0] 41 | 42 | - shape: {name: 'linestring',vertices: [[4, 0], [6, 7]]} # vertices 43 | state: [0, 0, 0] 44 | unobstructed: True 45 | plot: 46 | obj_linestyle: '--' 47 | 48 | -------------------------------------------------------------------------------- /irsim/usage/05lidar_world/lidar_world_laser_color.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | import numpy as np 3 | 4 | env = irsim.make() 5 | 6 | for i in range(300): 7 | 8 | env.step() 9 | env.render(0.05) 10 | 11 | indices = np.arange(0, i) 12 | env.robot.set_laser_color(indices, 'cyan', 0.0) 13 | 14 | if env.done(): 15 | break 16 | 17 | env.end(3) 18 | -------------------------------------------------------------------------------- /irsim/usage/05lidar_world/lidar_world_laser_color.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive', 'unobstructed_obstacles' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'diff'} # omni, diff, acker 12 | shape: {name: 'circle', radius: 0.2} # radius 13 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 14 | state: [1, 1, 0] 15 | goal: [9, 9, 0] 16 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 17 | behavior: {name: 'dash'} # move toward to the goal directly 18 | 19 | sensors: 20 | - name: 'lidar2d' 21 | range_min: 0 22 | range_max: 5 23 | angle_range: 3.14 # 4.7123 24 | number: 200 25 | noise: False 26 | std: 0.2 27 | angle_std: 0.2 28 | offset: [0, 0, 0] 29 | alpha: 0.5 30 | 31 | obstacle: 32 | - shape: {name: 'circle', radius: 1.0} # radius 33 | state: [5, 5, 0] 34 | 35 | 36 | -------------------------------------------------------------------------------- /irsim/usage/05lidar_world/lidar_world_noise.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive', 'unobstructed_obstacles' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'diff'} # omni, diff, acker 12 | shape: {name: 'circle', radius: 0.2} # radius 13 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 14 | state: [1, 1, 0] 15 | goal: [9, 9, 0] 16 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 17 | behavior: {name: 'dash'} # move toward to the goal directly 18 | 19 | sensors: 20 | - type: 'lidar2d' 21 | range_min: 0 22 | range_max: 5 23 | angle_range: 3.14 # 4.7123 24 | number: 200 25 | noise: True 26 | std: 0.1 27 | angle_std: 0.2 28 | offset: [0, 0, 0] 29 | alpha: 0.3 30 | 31 | 32 | obstacle: 33 | - shape: {name: 'circle', radius: 1.0} # radius 34 | state: [5, 5, 0] 35 | 36 | - shape: {name: 'rectangle', length: 1.5, width: 1.2} # length, width 37 | state: [6, 5, 1] 38 | 39 | - shape: {name: 'linestring',vertices: [[10, 5], [4, 0], [6, 7]]} # vertices 40 | state: [0, 0, 0] 41 | -------------------------------------------------------------------------------- /irsim/usage/06multi_objects_world/multi_objects_large.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | import time 3 | env = irsim.make(save_ani=False, full=False) 4 | 5 | for i in range(100): 6 | start_time = time.time() 7 | env.step() 8 | env.render(0.01) 9 | end_time = time.time() 10 | print(f"Time taken: {end_time - start_time} seconds") 11 | 12 | if env.done(): 13 | break 14 | 15 | env.end(5) 16 | -------------------------------------------------------------------------------- /irsim/usage/06multi_objects_world/multi_objects_large.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | 11 | robot: 12 | - number: 50 13 | distribution: {name: 'circle', radius: 20.0, center: [25, 25]} 14 | kinematics: {name: 'diff'} 15 | shape: 16 | - {name: 'circle', radius: 0.2} # radius 17 | # behavior: {name: 'rvo', vxmax: 1.0, vymax: 1.0, acce: 1.5, factor: 1.5} 18 | behavior: {name: 'dash'} 19 | vel_min: [-3, -3.0] 20 | vel_max: [3, 3.0] 21 | color: ['royalblue', 'red', 'green', 'orange', 'purple', 'yellow', 'cyan', 'magenta', 'lime', 'pink', 'brown'] 22 | arrive_mode: position 23 | goal_threshold: 0.15 24 | plot: 25 | show_trail: true 26 | show_goal: true 27 | trail_fill: true 28 | trail_alpha: 0.2 29 | show_trajectory: false 30 | 31 | sensors: 32 | - name: 'lidar2d' 33 | range_min: 0 34 | range_max: 5 35 | angle_range: 3.14 # 4.7123 36 | number: 200 37 | noise: False 38 | std: 0.2 39 | angle_std: 0.2 40 | offset: [0.1, 0.1, 0.5] 41 | alpha: 0.3 42 | 43 | -------------------------------------------------------------------------------- /irsim/usage/06multi_objects_world/multi_objects_world.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make('multi_objects_world.yaml') 4 | 5 | for i in range(300): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end(3) 14 | 15 | -------------------------------------------------------------------------------- /irsim/usage/06multi_objects_world/multi_objects_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive', 'unobstructed_obstacles' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | 11 | robot: 12 | - number: 2 13 | distribution: {name: 'manual'} 14 | kinematics: {name: 'diff'} 15 | shape: 16 | - {name: 'circle', radius: 0.2} # radius 17 | state: 18 | - [1, 1, 0] 19 | - [2, 1, 0] 20 | goal: 21 | - [9, 9, 0] 22 | - [9, 2, 0] 23 | behavior: 24 | - {name: 'dash'} # move toward to the goal directly 25 | - {name: 'dash'} # move toward to the goal directly 26 | color: 27 | - 'royalblue' 28 | - 'red' 29 | plot: 30 | show_text: True 31 | 32 | - number: 4 33 | distribution: {name: 'random'} 34 | kinematics: {name: 'diff'} 35 | shape: 36 | - {name: 'circle', radius: 0.2} # radius 37 | behavior: 38 | - {name: 'dash'} # move toward to the goal directly 39 | color: 40 | - 'pink' 41 | 42 | plot: 43 | goal_color: 'purple' 44 | plot: 45 | show_text: True 46 | 47 | 48 | obstacle: 49 | - number: 10 50 | distribution: {name: 'manual'} 51 | state: [[4, 8], [31, 38], [10, 20], [41, 25], [20, 13], [16, 26], [10, 24], [18, 20], [16, 26], [19, 26], [10, 30]] 52 | shape: 53 | - {name: 'circle', radius: 0.4} # radius 54 | - {name: 'circle', radius: 0.1} # radius 55 | color: 'k' 56 | plot: 57 | show_text: True 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /irsim/usage/07render_world/render.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make(save_ani=True, display=False) 4 | 5 | for i in range(300): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | if env.done(): 11 | break 12 | 13 | # env.end(3) 14 | env.end(suffix='.mp4') 15 | -------------------------------------------------------------------------------- /irsim/usage/07render_world/render.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive', 'unobstructed_obstacles' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | plot: 11 | no_axis: False 12 | 13 | robot: 14 | - kinematics: {name: 'diff'} # omni, diff, acker 15 | shape: {name: 'circle', radius: 0.2} # radius 16 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 17 | state: [1, 1, 0] 18 | goal: [9, 4, 0] 19 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 20 | behavior: {name: 'dash'} # move toward to the goal directly 21 | 22 | - kinematics: {name: 'diff'} # omni, diff, acker 23 | shape: {name: 'circle', radius: 0.2} # radius 24 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 25 | state: [5, 1, 0] 26 | goal: [2, 6, 0] 27 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 28 | behavior: {name: 'dash'} # move toward to the goal directly 29 | color: 'royalblue' 30 | plot: {show_trajectory: True, show_trail: True, trail_fill: True, trail_alpha: 0.2} 31 | 32 | 33 | obstacle: 34 | - number: 10 35 | distribution: {name: 'manual'} 36 | shape: 37 | - {name: 'circle', radius: 1.5} # radius 38 | - {name: 'circle', radius: 1.0} # radius 39 | 40 | state: [[20, 34], [31, 38], [10, 20], [41, 25], [20, 13], [16, 26], [10, 24], [18, 20], [16, 26], [19, 26], [10, 30]] 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | # - shape: {name: 'circle', radius: 0.4} # radius 49 | # state: [1, 1, 0] -------------------------------------------------------------------------------- /irsim/usage/07render_world/render_save_figure.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make('render.yaml', save_ani=False, display=False) 4 | 5 | for i in range(300): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.save_figure('render2.pdf') 14 | env.end(3) 15 | -------------------------------------------------------------------------------- /irsim/usage/08random_obstacle/dynamic_random.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make() 4 | 5 | for i in range(1000): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end() -------------------------------------------------------------------------------- /irsim/usage/08random_obstacle/dynamic_random.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | control_mode: 'keyboard' 5 | 6 | robot: 7 | - kinematics: {name: 'acker'} 8 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 9 | state: [5, 5, 0, 0] 10 | goal: [40, 40, 0] 11 | vel_max: [4, 1] 12 | 13 | sensors: 14 | - type: 'lidar2d' 15 | range_min: 0 16 | range_max: 20 17 | angle_range: 3.14 18 | number: 100 19 | 20 | plot: 21 | show_trajectory: True 22 | 23 | obstacle: 24 | 25 | - number: 20 26 | kinematics: {name: 'omni'} 27 | distribution: {name: 'random', range_low: [10, 10, -3.14], range_high: [40, 40, 3.14]} 28 | behavior: {name: 'rvo', wander: True, range_low: [10, 10, -3.14], range_high: [40, 40, 3.14], vxmax: 2, vymax: 2, factor: 3.0} 29 | vel_max: [4, 4] 30 | vel_min: [-4, -4] 31 | shape: 32 | - {name: 'circle', radius: 1.0, random_shape: True} 33 | - {name: 'polygon', random_shape: true, avg_radius_range: [0.5, 2.0], irregularity_range: [0, 0.4], spikeyness_range: [0, 0.4], num_vertices_range: [4, 6]} -------------------------------------------------------------------------------- /irsim/usage/08random_obstacle/random_obstacle.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make('random_obstacle.yaml', save_ani=False, full=False) 4 | 5 | for i in range(300): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end(3, rm_fig_path=False) 14 | -------------------------------------------------------------------------------- /irsim/usage/08random_obstacle/random_obstacle.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'acker'} # omni, diff, acker 12 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 13 | state: [5, 5, 0, 0] 14 | goal: [40, 40, 0] 15 | vel_max: [4, 1] 16 | behavior: {name: 'dash'} # move toward to the goal directly 17 | 18 | sensors: 19 | - type: 'lidar2d' 20 | range_min: 0 21 | range_max: 20 22 | angle_range: 3.14 23 | number: 100 24 | noise: False 25 | std: 1 26 | angle_std: 0.2 27 | offset: [0, 0, 0] 28 | alpha: 0.4 29 | 30 | obstacle: 31 | - number: 10 32 | distribution: {name: 'manual'} 33 | shape: 34 | - {name: 'polygon', random_shape: true, center_range: [5, 10, 40, 30], avg_radius_range: [0.5, 2], irregularity_range: [0, 1], spikeyness_range: [0, 1], num_vertices_range: [4, 5]} # radius 35 | 36 | - number: 10 37 | distribution: {name: 'random', range_low: [10, 10, -3.14], range_high: [40, 40, 3.14]} 38 | shape: 39 | - {name: 'circle', radius: 0.4} # radius -------------------------------------------------------------------------------- /irsim/usage/09keyboard_control/keyboard_control.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make('keyboard_control.yaml', save_ani=False, full=False) 4 | 5 | for i in range(300): 6 | 7 | env.step() 8 | env.render(0.05, show_goal=False, show_trajectory=True) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end(3) 14 | -------------------------------------------------------------------------------- /irsim/usage/09keyboard_control/keyboard_control.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'keyboard' # 'keyboard', 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'acker'} # omni, diff, acker 12 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 13 | state: [5, 5, 0, 0] 14 | goal: [40, 40, 0] 15 | vel_max: [4, 1] 16 | behavior: {name: 'dash'} # move toward to the goal directly 17 | plot: 18 | show_trail: True 19 | traj_color: 'g' 20 | 21 | sensors: 22 | - type: 'lidar2d' 23 | range_min: 0 24 | range_max: 20 25 | angle_range: 3.14 26 | number: 100 27 | noise: False 28 | std: 1 29 | angle_std: 0.2 30 | offset: [0, 0, 0] 31 | alpha: 0.4 32 | 33 | 34 | obstacle: 35 | - number: 10 36 | distribution: {name: 'manual'} 37 | shape: 38 | - {name: 'polygon', random_shape: true, center_range: [5, 10, 40, 30], avg_radius_range: [0.5, 2]} # radius 39 | 40 | -------------------------------------------------------------------------------- /irsim/usage/10grid_map/grid_map.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make(save_ani=False, full=False) 4 | 5 | for i in range(1000): 6 | 7 | env.step() 8 | env.render() 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end(5) -------------------------------------------------------------------------------- /irsim/usage/10grid_map/grid_map.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'keyboard' # 'keyboard', 'auto' 9 | obstacle_map: 'cave.png' 10 | mdownsample: 2 11 | 12 | robot: 13 | - kinematics: {name: 'acker'} # omni, diff, acker 14 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 15 | state: [5, 5, 0, 0] 16 | goal: [40, 40, 0] 17 | vel_max: [4, 1] 18 | behavior: {name: 'dash'} # move toward to the goal directly 19 | plot: 20 | show_trail: True 21 | traj_color: 'g' 22 | show_trajectory: True 23 | show_goal: False 24 | 25 | sensors: 26 | - type: 'lidar2d' 27 | range_min: 0 28 | range_max: 20 29 | angle_range: 3.14 30 | number: 100 31 | noise: False 32 | std: 1 33 | angle_std: 0.2 34 | offset: [0, 0, 0] 35 | alpha: 0.4 36 | 37 | 38 | obstacle: 39 | - number: 10 40 | distribution: {name: 'manual'} 41 | shape: 42 | - {name: 'polygon', random_shape: true, center_range: [5, 10, 40, 30], avg_radius_range: [0.5, 2]} # radius 43 | 44 | -------------------------------------------------------------------------------- /irsim/usage/10grid_map/grid_map_hm3d.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | import time 3 | env = irsim.make(save_ani=False, full=False) 4 | 5 | for i in range(1000): 6 | 7 | env.step() 8 | env.render() 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end(5) -------------------------------------------------------------------------------- /irsim/usage/10grid_map/grid_map_hm3d.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 80 # the height of the world 3 | width: 80 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'keyboard' # 'keyboard', 'auto' 9 | obstacle_map: 'hm3d_2.png' # hm3d_1.png, hm3d_2.png, hm3d_3.png, hm3d_4.png, hm3d_5.png, hm3d_6.png, hm3d_7.png, hm3d_8.png, hm3d_9.png 10 | mdownsample: 2 11 | 12 | robot: 13 | - kinematics: {name: 'acker'} # omni, diff, acker 14 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 15 | state: [63, 10, 1.57] 16 | goal: [63, 40, 1.57] 17 | vel_max: [6, 1] 18 | behavior: {name: 'dash'} # move toward to the goal directly 19 | plot: 20 | show_trail: True 21 | traj_color: 'g' 22 | show_trajectory: True 23 | show_goal: False 24 | 25 | sensors: 26 | - type: 'lidar2d' 27 | range_min: 0 28 | range_max: 15 29 | angle_range: 3.14 30 | number: 100 31 | noise: False 32 | std: 1 33 | angle_std: 0.2 34 | offset: [0, 0, 0] 35 | alpha: 0.4 36 | 37 | keyboard: 38 | vel_max: [6, 1] 39 | 40 | -------------------------------------------------------------------------------- /irsim/usage/11collision_avoidance/collision_avoidance.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make(save_ani=False, full=False) 4 | 5 | for i in range(1000): 6 | 7 | env.step() 8 | env.render(0.01) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end(5) 14 | -------------------------------------------------------------------------------- /irsim/usage/11collision_avoidance/collision_avoidance.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | 11 | robot: 12 | - number: 10 13 | distribution: {name: 'circle', radius: 4.0, center: [5, 5]} 14 | kinematics: {name: 'diff'} 15 | shape: 16 | - {name: 'circle', radius: 0.2} # radius 17 | behavior: {name: 'rvo', vxmax: 1.0, vymax: 1.0, acce: 1.0, factor: 2.0, neighbor_threshold: 5.0} 18 | vel_min: [-3, -3.0] 19 | vel_max: [3, 3.0] 20 | color: ['royalblue', 'red', 'green', 'orange', 'purple', 'yellow', 'cyan', 'magenta', 'lime', 'pink', 'brown'] 21 | arrive_mode: position 22 | goal_threshold: 0.2 23 | plot: 24 | show_trail: true 25 | show_goal: true 26 | trail_fill: true 27 | trail_alpha: 0.2 28 | show_trajectory: false 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /irsim/usage/11collision_avoidance/collision_avoidance_large.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | import time 3 | env = irsim.make(save_ani=False, full=False) 4 | 5 | for i in range(1000): 6 | 7 | env.step() 8 | env.render(0.01) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end(5) 14 | -------------------------------------------------------------------------------- /irsim/usage/11collision_avoidance/collision_avoidance_large.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | 11 | robot: 12 | - number: 50 13 | distribution: {name: 'circle', radius: 20.0, center: [25, 25]} 14 | kinematics: {name: 'diff'} 15 | shape: 16 | - {name: 'circle', radius: 0.2} # radius 17 | behavior: {name: 'rvo', vxmax: 1.0, vymax: 1.0, acce: 1.5, factor: 1.5} 18 | vel_min: [-3, -3.0] 19 | vel_max: [3, 3.0] 20 | color: ['royalblue', 'red', 'green', 'orange', 'purple', 'yellow', 'cyan', 'magenta', 'lime', 'pink', 'brown'] 21 | arrive_mode: position 22 | goal_threshold: 0.15 23 | plot: 24 | show_trail: true 25 | show_goal: true 26 | trail_fill: true 27 | trail_alpha: 0.2 28 | show_trajectory: false 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /irsim/usage/12dynamic_obstacle/dynamic_obstacle.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make() 4 | 5 | for i in range(3000): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | if env.done(): 11 | break 12 | 13 | env.end() 14 | -------------------------------------------------------------------------------- /irsim/usage/12dynamic_obstacle/dynamic_obstacle.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'unobstructed_obstacles' # 'stop', 'unobstructed', 'reactive' 8 | 9 | robot: 10 | - kinematics: {name: 'diff'} # omni, diff, acker 11 | shape: {name: 'circle', radius: 0.2} # radius 12 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 13 | state: [1, 1, 0] 14 | goal: [9, 9, 0] 15 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 16 | behavior: {name: 'dash'} # move toward to the goal directly 17 | plot: 18 | show_trajectory: True 19 | 20 | 21 | obstacle: 22 | - kinematics: {name: 'omni'} 23 | behavior: {name: 'dash'} 24 | shape: {name: 'circle', radius: 1.0} # radius 25 | state: [5, 5, 0] 26 | 27 | - shape: {name: 'rectangle', length: 1.5, width: 1.2} # radius 28 | state: [6, 5, 1] 29 | 30 | - shape: {name: 'linestring', vertices: [[5, 5], [4, 0], [1, 6]] } # vertices 31 | state: [0, 0, 0] 32 | unobstructed: True 33 | plot: 34 | obj_linestyle: '--' 35 | -------------------------------------------------------------------------------- /irsim/usage/13custom_behavior/custom_behavior.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | ''' 4 | - Refer to the irsim/lib/behavior_methods.py file for the custom behavior design. 5 | - custom_behavior_methods.py is the designated module name. It should be placed in the same directory as the current implementation script. 6 | - The behavior names defined in custom_behavior_methods.py (e.g., dash_custom) must match the behavior names specified in the YAML file. 7 | ''' 8 | 9 | env = irsim.make() 10 | env.load_behavior("custom_behavior_methods") 11 | 12 | for i in range(1000): 13 | 14 | env.step() 15 | env.render(0.01) 16 | 17 | if env.done(): 18 | break 19 | 20 | env.end(5) 21 | -------------------------------------------------------------------------------- /irsim/usage/13custom_behavior/custom_behavior.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | 11 | robot: 12 | - number: 3 13 | distribution: {name: 'circle', radius: 4.0, center: [5, 5]} 14 | kinematics: {name: 'diff'} 15 | shape: 16 | - {name: 'circle', radius: 0.2} # radius 17 | behavior: {name: 'rvo', vxmax: 1.0, vymax: 1.0, acce: 1.0, factor: 1.0} 18 | vel_min: [-3, -3.0] 19 | vel_max: [3, 3.0] 20 | color: ['royalblue', 'red', 'green'] 21 | arrive_mode: position 22 | goal_threshold: 0.2 23 | plot: 24 | show_trail: true 25 | show_goal: true 26 | trail_fill: true 27 | trail_alpha: 0.2 28 | show_trajectory: false 29 | 30 | - number: 2 31 | distribution: {name: 'circle', radius: 4.0, center: [5, 5]} 32 | kinematics: {name: 'diff'} 33 | shape: 34 | - {name: 'circle', radius: 0.2} # radius 35 | behavior: {name: 'dash', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 1.0} 36 | vel_min: [-3, -3.0] 37 | vel_max: [3, 3.0] 38 | color: ['orange', 'purple'] 39 | arrive_mode: position 40 | goal_threshold: 0.2 41 | plot: 42 | show_trail: true 43 | show_goal: true 44 | trail_fill: true 45 | trail_alpha: 0.2 46 | show_trajectory: false 47 | 48 | - number: 2 49 | distribution: {name: 'random'} 50 | kinematics: {name: 'diff'} 51 | shape: 52 | - {name: 'circle', radius: 0.2} # radius 53 | behavior: {name: 'dash_custom'} 54 | vel_min: [-3, -3.0] 55 | vel_max: [3, 3.0] 56 | color: ['pink', 'brown'] 57 | arrive_mode: position 58 | goal_threshold: 0.2 59 | plot: 60 | show_trail: true 61 | show_goal: true 62 | trail_fill: true 63 | trail_alpha: 0.2 64 | show_trajectory: false 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /irsim/usage/13custom_behavior/custom_behavior_methods.py: -------------------------------------------------------------------------------- 1 | from irsim.lib import register_behavior 2 | from irsim.util.util import relative_position, WrapToPi 3 | import numpy as np 4 | 5 | 6 | @register_behavior("diff", "dash_custom") 7 | def beh_diff_dash(ego_object, external_objects=[], **kwargs): 8 | 9 | print("This is a custom behavior example for differential drive with dash2") 10 | 11 | state = ego_object.state 12 | goal = ego_object.goal 13 | goal_threshold = ego_object.goal_threshold 14 | _, max_vel = ego_object.get_vel_range() 15 | angle_tolerance = kwargs.get("angle_tolerance", 0.1) 16 | 17 | behavior_vel = DiffDash2(state, goal, max_vel, goal_threshold, angle_tolerance) 18 | 19 | return behavior_vel 20 | 21 | 22 | def DiffDash2(state, goal, max_vel, goal_threshold=0.1, angle_tolerance=0.2): 23 | """ 24 | Calculate the differential drive velocity to reach a goal. 25 | 26 | Args: 27 | state (np.array): Current state [x, y, theta] (3x1). 28 | goal (np.array): Goal position [x, y, theta] (3x1). 29 | max_vel (np.array): Maximum velocity [linear, angular] (2x1). 30 | goal_threshold (float): Distance threshold to consider goal reached (default 0.1). 31 | angle_tolerance (float): Allowable angular deviation (default 0.2). 32 | 33 | Returns: 34 | np.array: Velocity [linear, angular] (2x1). 35 | """ 36 | distance, radian = relative_position(state, goal) 37 | 38 | if distance < goal_threshold: 39 | return np.zeros((2, 1)) 40 | 41 | diff_radian = WrapToPi(radian - state[2, 0]) 42 | linear = max_vel[0, 0] * np.cos(diff_radian) 43 | 44 | if abs(diff_radian) < angle_tolerance: 45 | angular = 0 46 | else: 47 | angular = max_vel[1, 0] * np.sign(diff_radian) 48 | 49 | return np.array([[linear], [angular]]) -------------------------------------------------------------------------------- /irsim/usage/14world_3d_plot/car_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | 8 | robot: 9 | - kinematics: {name: 'acker'} # omni, diff, acker 10 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 11 | state: [1, 1, 0, 0] 12 | goal: [40, 40, 0] 13 | vel_max: [4, 1] 14 | behavior: {name: 'dash'} # move toward to the goal directly 15 | plot: 16 | show_trajectory: True 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /irsim/usage/14world_3d_plot/robot_omni_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | 8 | robot: 9 | - kinematics: {name: 'omni'} # omni, diff, acker 10 | # shape: {name: 'circle', radius: 0.2} # radius 11 | shape: {name: 'rectangle', length: 0.3, width: 1.0} 12 | state: [1, 1, 0] 13 | goal: [9, 9, 0] 14 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 15 | behavior: {name: 'dash'} # move toward to the goal directly 16 | color: 'g' 17 | state_dim: 3 18 | plot: 19 | show_trajectory: True 20 | # description: 'diff_robot0.png' 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /irsim/usage/14world_3d_plot/robot_polygon_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | 8 | robot: 9 | kinematics: {name: 'diff'} # omni, diff, acker 10 | shape: {name: 'polygon', vertices: [[0.1, 0.2], [-0.1, 0.3], [0, -0.3], [0.3, -0.2]]} # radius 11 | behavior: {name: 'dash'} # move toward to the goal directly 12 | state: [1, 1, 0] 13 | goal: [9, 9, 0] 14 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 15 | color: 'g' 16 | plot: 17 | show_trajectory: True 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /irsim/usage/14world_3d_plot/robot_world_3d.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | # env = irsim.make('car_world.yaml', projection='3d') 4 | env = irsim.make('robot_world_3d.yaml', projection='3d') 5 | 6 | for i in range(1000): 7 | 8 | env.step() 9 | env.render(0.05) 10 | 11 | # if env.done(): 12 | # break 13 | 14 | env.end() 15 | -------------------------------------------------------------------------------- /irsim/usage/14world_3d_plot/robot_world_3d.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | 8 | robot: 9 | kinematics: {name: 'diff'} # omni, diff, acker 10 | shape: {name: 'circle', radius: 0.2} # radius 11 | # shape: {name: 'rectangle', length: 0.3, width: 1.0} 12 | state: [1, 1, 0] 13 | goal: [9, 9, 0] 14 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 15 | behavior: {name: 'dash'} # move toward to the goal directly 16 | color: 'g' 17 | plot: 18 | show_trajectory: True 19 | show_trail: True 20 | # description: 'diff_robot0.png' 21 | 22 | sensors: 23 | - name: 'lidar2d' 24 | range_min: 0 25 | range_max: 5 26 | angle_range: 3.14 # 4.7123 27 | number: 200 28 | noise: False 29 | std: 0.2 30 | angle_std: 0.2 31 | alpha: 0.3 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /irsim/usage/15fov_world/fov_world.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make() 4 | 5 | for i in range(200): 6 | 7 | env.step() 8 | 9 | for obs in env.obstacle_list: 10 | if obs.fov_detect_object(env.robot): 11 | print(f'The robot is in the FOV of the {obs.name}. The parameters of this obstacle are: state [x, y, theta]: {obs.state.flatten()}, velocity [linear, angular]: {obs.velocity.flatten()}, fov in radian: {obs.fov}.') 12 | 13 | env.render(figure_kwargs={'dpi': 100}) 14 | 15 | env.end() 16 | -------------------------------------------------------------------------------- /irsim/usage/15fov_world/fov_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | # control_mode: 'keyboard' # 'keyboard', 'auto' 10 | 11 | robot: 12 | - kinematics: {name: 'diff'} # omni, diff, acker 13 | shape: {name: 'circle', radius: 0.4} 14 | state: [10, 10, 0, 0] 15 | goal: [45, 45, 0] 16 | goal_threshold: 0.4 17 | vel_max: [3, 1] 18 | vel_min: [-3, -1] 19 | behavior: {name: 'dash', wander: True, range_low: [15, 15, -3.14], range_high: [35, 35, 3.14]} 20 | plot: 21 | show_goal: True 22 | show_trajectory: True 23 | 24 | obstacle: 25 | - number: 10 26 | distribution: {name: 'random', range_low: [10, 10, -3.14], range_high: [40, 40, 3.14]} 27 | kinematics: {name: 'diff'} 28 | behavior: {name: 'rvo', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 2.0, mode: 'vo', wander: True, range_low: [15, 15, -3.14], range_high: [35, 35, 3.14], target_roles: 'all'} 29 | vel_max: [3, 3.14] 30 | vel_min: [-3, -3.14] 31 | shape: 32 | - {name: 'circle', radius: 0.5} # radius 33 | fov: 1.57 34 | fov_radius: 5.0 35 | plot: 36 | show_fov: True 37 | show_arrow: True 38 | arrow_length: 0.8 39 | -------------------------------------------------------------------------------- /irsim/usage/15fov_world/fov_world_multi.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make() 4 | 5 | for i in range(200): 6 | 7 | env.step() 8 | 9 | for obs in env.obstacle_list: 10 | if obs.fov_detect_object(env.robot): 11 | print(f'The robot is in the FOV of the {obs.name}. The parameters of this obstacle are: state [x, y, theta]: {obs.state.flatten()}, velocity [linear, angular]: {obs.velocity.flatten()}, fov in radian: {obs.fov}.') 12 | 13 | env.render(figure_kwargs={'dpi': 100}) 14 | 15 | env.end() 16 | -------------------------------------------------------------------------------- /irsim/usage/15fov_world/fov_world_multi.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | # control_mode: 'auto' # 'keyboard', 'auto' 9 | control_mode: 'keyboard' # 'keyboard', 'auto' 10 | 11 | robot: 12 | - number: 3 13 | kinematics: {name: 'diff'} # omni, diff, acker 14 | shape: {name: 'circle', radius: 0.4} 15 | state: [[10, 10, 0], [20, 20, 0], [30, 30, 0]] 16 | goal: [45, 45, 0] 17 | goal_threshold: 0.4 18 | vel_max: [3, 1] 19 | vel_min: [-3, -1] 20 | # behavior: {name: 'dash', wander: True, range_low: [15, 15, -3.14], range_high: [35, 35, 3.14]} 21 | plot: 22 | show_goal: True 23 | show_trajectory: True 24 | show_text: True 25 | 26 | obstacle: 27 | - number: 10 28 | distribution: {name: 'random', range_low: [10, 10, -3.14], range_high: [40, 40, 3.14]} 29 | kinematics: {name: 'diff'} 30 | behavior: {name: 'rvo', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 2.0, mode: 'vo', wander: True, range_low: [15, 15, -3.14], range_high: [35, 35, 3.14], target_roles: 'all'} 31 | vel_max: [3, 3.14] 32 | vel_min: [-3, -3.14] 33 | shape: 34 | - {name: 'circle', radius: 0.5} # radius 35 | fov: 1.57 36 | fov_radius: 5.0 37 | plot: 38 | show_fov: True 39 | show_arrow: True 40 | arrow_length: 0.8 41 | -------------------------------------------------------------------------------- /irsim/usage/16noise_world/noise_world.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | env = irsim.make(save_ani=False) 4 | 5 | for i in range(200): 6 | 7 | env.step() 8 | env.render(0.05) 9 | 10 | env.end(3) 11 | -------------------------------------------------------------------------------- /irsim/usage/16noise_world/noise_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 3 | width: 10 4 | step_time: 0.1 5 | sample_time: 0.1 6 | offset: [0, 0] 7 | collision_mode: 'stop' 8 | control_mode: 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'diff', noise: True, alpha: [0.1, 0, 0, 0.3]} 12 | shape: {name: 'circle', radius: 0.2} 13 | state: [1, 1, 0] 14 | goal: [9, 9, 0] 15 | behavior: {name: 'dash', wander: True} 16 | 17 | plot: 18 | show_trajectory: True 19 | 20 | sensors: 21 | - type: 'lidar2d' 22 | range_min: 0 23 | range_max: 5 24 | angle_range: 3.14 # 4.7123 25 | number: 200 26 | noise: True 27 | std: 0.1 28 | angle_std: 0.2 29 | offset: [0, 0, 0] 30 | alpha: 0.3 31 | 32 | 33 | obstacle: 34 | - shape: {name: 'circle', radius: 1.0} 35 | state: [5, 5, 0] 36 | 37 | -------------------------------------------------------------------------------- /irsim/version.py: -------------------------------------------------------------------------------- 1 | import importlib.metadata 2 | 3 | __version__ = importlib.metadata.version("ir-sim") 4 | -------------------------------------------------------------------------------- /irsim/world/__init__.py: -------------------------------------------------------------------------------- 1 | from irsim.world.world import World 2 | from irsim.world.sensors.sensor_factory import SensorFactory 3 | 4 | 5 | from irsim.world.object_base import ObjectBase 6 | from irsim.world.robots.robot_diff import RobotDiff 7 | from irsim.world.robots.robot_acker import RobotAcker 8 | from irsim.world.robots.robot_omni import RobotOmni 9 | 10 | # from irsim.world.object_base_3d import ObjectBase3D 11 | # from irsim.world.robots.robot_rigid3d import RobotRigid3D 12 | 13 | from irsim.world.obstacles.obstacle_diff import ObstacleDiff 14 | from irsim.world.obstacles.obstacle_omni import ObstacleOmni 15 | from irsim.world.obstacles.obstacle_acker import ObstacleAcker 16 | 17 | from irsim.world.map.obstacle_map import ObstacleMap 18 | from irsim.world.obstacles.obstacle_static import ObjectStatic 19 | 20 | 21 | from irsim.world.object_factory import ObjectFactory 22 | 23 | -------------------------------------------------------------------------------- /irsim/world/arm_base.py: -------------------------------------------------------------------------------- 1 | # import numpy as np 2 | 3 | # class arm: 4 | # pass -------------------------------------------------------------------------------- /irsim/world/description/car_blue.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/description/car_blue.png -------------------------------------------------------------------------------- /irsim/world/description/car_green.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/description/car_green.png -------------------------------------------------------------------------------- /irsim/world/description/car_red.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/description/car_red.png -------------------------------------------------------------------------------- /irsim/world/description/diff_robot0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/description/diff_robot0.png -------------------------------------------------------------------------------- /irsim/world/description/diff_robot1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/description/diff_robot1.png -------------------------------------------------------------------------------- /irsim/world/map/__init__.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Map: 4 | def __init__(self, width: int = 10, height: int = 10, resolution: float = 0.1, obstacle_list: list = [], grid: np.ndarray = None): 5 | 6 | ''' 7 | Map class for storing map data and navigation information 8 | 9 | Args: 10 | width (int): width of the map 11 | height (int): height of the map 12 | resolution (float): resolution of the map 13 | obstacle_list (list): list of obstacle objects for collision detection 14 | grid (np.ndarray): grid map data for collision detection. 15 | ''' 16 | 17 | self.width = width 18 | self.height = height 19 | self.resolution = resolution 20 | self.obstacle_list = obstacle_list 21 | self.grid = grid 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/README.md: -------------------------------------------------------------------------------- 1 | # Binary Map Generator Configuration 2 | 3 | The Binary Map Generator converts 3D scene datasets into 2D binary occupancy maps. Configure it by specifying the input scene file and output path directly in the Python script. 4 | 5 | ## Dependencies 6 | 7 | - [habitat-sim](https://github.com/facebookresearch/habitat-sim) (for NavMesh extraction) 8 | - `shapely`, `matplotlib`, `numpy` (for geometry processing and visualization) 9 | 10 | ## Configuration Parameters 11 | 12 | The main script (`binary_map_generator.py`) uses the following parameters: 13 | 14 | | Parameter | Description | 15 | |--------------|-----------------------------------------------------------------------------| 16 | | `scene_name` | Identifier for the scene (e.g., `"00824-Dd4bFSTQ8gi"`). | 17 | | `scene_path` | Path to the 3D scene file (e.g., `"/path/to/scenes/00824-Dd4bFSTQ8gi.glb"`).| 18 | 19 | ## Usage 20 | 21 | 1. Set up [Habitat-Sim](https://github.com/facebookresearch/habitat-sim) (Follow the official Habitat-Sim installation guide) and download the 3D scene datasets like [HM3D](https://aihabitat.org/datasets/hm3d/), [MatterPort3D](https://niessner.github.io/Matterport/), [Gibson](http://gibsonenv.stanford.edu/database/), etc. 22 | 2. Configure the parameters in the script (`binary_map_generator.py`): 23 | ```python 24 | scene_name = "your-scene-id" # E.g., "00824-Dd4bFSTQ8gi" 25 | scene_path = f"/path/to/your/scenes/{scene_name}.glb" 26 | ``` 27 | 3. Generate binary maps: 28 | 29 | ```bash 30 | python binary_map_generator.py 31 | ``` 32 | 33 | # hm3d Real Maps vs. Binary Maps 34 | 35 | | Real Map | Binary Map | 36 | |:--------:|:----------:| 37 | | Real map 1
hm3d Real Map 1 (00813-svBbv1Pavdk) | Binary map 1
hm3d Binary Map 1 (00813-svBbv1Pavdk) | 38 | | Real map 2
hm3d Real Map 2 (00824-Dd4bFSTQ8gi) | Binary map 2
hm3d Binary Map 2 (00824-Dd4bFSTQ8gi) | 39 | | Real map 3
hm3d Real Map 3 (00827-BAbdmeyTvMZ) | Binary map 3
hm3d Binary Map 3 (00827-BAbdmeyTvMZ) | 40 | | Real map 4
hm3d Real Map 4 (00829-QaLdnwvtxbs) | Binary map 4
hm3d Binary Map 4 (00829-QaLdnwvtxbs) | 41 | | Real map 5
hm3d Real Map 5 (00848-ziup5kvtCCR) | Binary map 5
hm3d Binary Map 5 (00848-ziup5kvtCCR) | 42 | | Real map 6
hm3d Real Map 6 (00853-5cdEh9F2hJL) | Binary map 6
hm3d Binary Map 6 (00853-5cdEh9F2hJL) | 43 | | Real map 7
hm3d Real Map 7 (00871-VBzV5z6i1WS) | Binary map 7
hm3d Binary Map 7 (00871-VBzV5z6i1WS) | 44 | | Real map 8
hm3d Real Map 8 (00876-mv2HUxq3B53) | Binary map 8
hm3d Binary Map 8 (00876-mv2HUxq3B53) | 45 | | Real map 9
hm3d Real Map 9 (00880-Nfvxx8J5NCo) | Binary map 9
hm3d Binary Map 9 (00880-Nfvxx8J5NCo) | 46 | 47 | ## Acknowledgments 48 | 49 | - [habitat-sim](https://github.com/facebookresearch/habitat-sim) 50 | - [hm3d dataset](https://github.com/matterport/habitat-matterport-3dresearch) -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_1.png -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_2.png -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_3.png -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_4.png -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_5.png -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_6.png -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_7.png -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_8.png -------------------------------------------------------------------------------- /irsim/world/map/binary_map_generator_hm3d/hm3d_9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/binary_map_generator_hm3d/hm3d_9.png -------------------------------------------------------------------------------- /irsim/world/map/cave.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/irsim/world/map/cave.png -------------------------------------------------------------------------------- /irsim/world/map/obstacle_map.py: -------------------------------------------------------------------------------- 1 | from irsim.world import ObjectBase 2 | from shapely.strtree import STRtree 3 | 4 | class ObstacleMap(ObjectBase): 5 | 6 | def __init__( 7 | self, 8 | shape={"name": "map", "reso": "0.1", "points": None}, 9 | color="k", 10 | static=True, 11 | **kwargs, 12 | ): 13 | super(ObstacleMap, self).__init__( 14 | shape=shape, 15 | role="obstacle", 16 | color=color, 17 | static=static, 18 | **kwargs, 19 | ) 20 | 21 | 22 | self.linestrings = [line for line in self.geometry.geoms] 23 | self.geometry_tree = STRtree(self.linestrings) 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /irsim/world/object_base_3d.py: -------------------------------------------------------------------------------- 1 | # from irsim.world import ObjectBase 2 | # import matplotlib as mpl 3 | # from matplotlib import transforms as mtransforms 4 | # from matplotlib import image 5 | # from irsim.global_param.path_param import path_manager 6 | # from math import pi, atan2, cos, sin 7 | # import mpl_toolkits.mplot3d.art3d as art3d 8 | # import numpy as np 9 | # from mpl_toolkits.mplot3d.art3d import Line3D 10 | 11 | # class ObjectBase3D(ObjectBase): 12 | 13 | # ''' 14 | # state: x, y, z, roll, pitch, yaw 15 | # vel: x_dot, y_dot, z_dot, roll_dot, pitch_dot, yaw_dot 16 | # ''' 17 | 18 | # vel_shape = (6, 1) 19 | # state_shape = (6, 1) 20 | 21 | # def __init__(self, state=[0, 0, 0, 0, 0, 0], **kwargs): 22 | # super().__init__(state=state, **kwargs) 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /irsim/world/obstacles/obstacle_acker.py: -------------------------------------------------------------------------------- 1 | from irsim.world import ObjectBase 2 | 3 | 4 | class ObstacleAcker(ObjectBase): 5 | def __init__(self, color="k", **kwargs): 6 | super(ObstacleAcker, self).__init__( color=color, role="obstacle", **kwargs 7 | ) 8 | -------------------------------------------------------------------------------- /irsim/world/obstacles/obstacle_diff.py: -------------------------------------------------------------------------------- 1 | from irsim.world import ObjectBase 2 | 3 | class ObstacleDiff(ObjectBase): 4 | def __init__(self, color="k", **kwargs): 5 | super(ObstacleDiff, self).__init__(color=color, role="obstacle", **kwargs 6 | ) 7 | -------------------------------------------------------------------------------- /irsim/world/obstacles/obstacle_omni.py: -------------------------------------------------------------------------------- 1 | from irsim.world import ObjectBase 2 | 3 | 4 | class ObstacleOmni(ObjectBase): 5 | def __init__( 6 | self, color="k", **kwargs 7 | ): 8 | super(ObstacleOmni, self).__init__( 9 | color=color, role="obstacle", **kwargs 10 | ) 11 | -------------------------------------------------------------------------------- /irsim/world/obstacles/obstacle_static.py: -------------------------------------------------------------------------------- 1 | from irsim.world import ObjectBase 2 | 3 | 4 | class ObjectStatic(ObjectBase): 5 | def __init__( 6 | self, color="k", role="obstacle", **kwargs 7 | ): 8 | super(ObjectStatic, self).__init__(color=color, role=role, **kwargs 9 | ) 10 | 11 | self.static=True 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /irsim/world/robots/robot_acker.py: -------------------------------------------------------------------------------- 1 | from irsim.world import ObjectBase 2 | 3 | 4 | class RobotAcker(ObjectBase): 5 | def __init__( 6 | self, color="y", state_dim=4, description="car_green.png", **kwargs 7 | ): 8 | super(RobotAcker, self).__init__( 9 | role="robot", 10 | color=color, 11 | state_dim=state_dim, 12 | description=description, 13 | **kwargs, 14 | ) 15 | 16 | assert ( 17 | state_dim >= 4 18 | ), "for ackermann robot, the state dimension should be greater than 4" 19 | 20 | -------------------------------------------------------------------------------- /irsim/world/robots/robot_diff.py: -------------------------------------------------------------------------------- 1 | from irsim.world import ObjectBase 2 | 3 | 4 | class RobotDiff(ObjectBase): 5 | def __init__( 6 | self, color="g", state_dim=3, **kwargs 7 | ): 8 | super(RobotDiff, self).__init__( 9 | role="robot", 10 | color=color, 11 | state_dim=state_dim, 12 | **kwargs, 13 | ) 14 | 15 | assert ( 16 | state_dim >= 3 17 | ), "for differential robot, the state dimension should be greater than 3" 18 | 19 | 20 | def _init_plot(self, ax, **kwargs): 21 | 22 | show_goal = self.plot_kwargs.get("show_goal", True) 23 | show_arrow = self.plot_kwargs.get("show_arrow", True) 24 | 25 | super()._init_plot(ax, show_goal=show_goal, show_arrow=show_arrow, **kwargs) 26 | -------------------------------------------------------------------------------- /irsim/world/robots/robot_omni.py: -------------------------------------------------------------------------------- 1 | from irsim.world import ObjectBase 2 | 3 | 4 | class RobotOmni(ObjectBase): 5 | def __init__( 6 | self, color="g", state_dim=3, **kwargs 7 | ): 8 | super(RobotOmni, self).__init__( 9 | role="robot", 10 | color=color, 11 | state_dim=state_dim, 12 | **kwargs, 13 | ) 14 | 15 | assert ( 16 | state_dim >= 2 17 | ), "for omni robot, the state dimension should be greater than 2" 18 | 19 | 20 | def _init_plot(self, ax, **kwargs): 21 | 22 | show_goal = self.plot_kwargs.get("show_goal", True) 23 | 24 | super()._init_plot(ax, show_goal=show_goal, **kwargs) 25 | -------------------------------------------------------------------------------- /irsim/world/robots/robot_rigid3d.py: -------------------------------------------------------------------------------- 1 | # from irsim.world import ObjectBase3D 2 | 3 | 4 | # class RobotRigid3D(ObjectBase3D): 5 | # def __init__( 6 | # self, color="g", state_dim=6, **kwargs 7 | # ): 8 | # super(RobotRigid3D, self).__init__( 9 | # role="robot", 10 | # color=color, 11 | # state_dim=state_dim, 12 | # **kwargs, 13 | # ) 14 | 15 | # assert ( 16 | # state_dim >= 6 17 | # ), "for rigid robot, the state dimension should be greater than 6" 18 | 19 | -------------------------------------------------------------------------------- /irsim/world/sensors/sensor_factory.py: -------------------------------------------------------------------------------- 1 | from irsim.world.sensors.lidar2d import Lidar2D 2 | 3 | 4 | class SensorFactory: 5 | 6 | def create_sensor(self, state, obj_id, **kwargs): 7 | 8 | sensor_type = kwargs.get("name", kwargs.get("type", "lidar2d")) 9 | 10 | if sensor_type == "lidar2d": 11 | return Lidar2D(state, obj_id, **kwargs) 12 | else: 13 | raise NotImplementedError(f"Sensor types {type} not implemented") 14 | 15 | -------------------------------------------------------------------------------- /irsim/world/world.py: -------------------------------------------------------------------------------- 1 | from irsim.util.util import file_check 2 | from irsim.global_param import world_param 3 | import numpy as np 4 | import matplotlib.image as mpimg 5 | from typing import Optional 6 | import os 7 | from irsim.global_param.path_param import path_manager as pm 8 | from irsim.world.map import Map 9 | 10 | class World: 11 | """ 12 | Represents the main simulation environment, managing objects and maps. 13 | 14 | Attributes: 15 | name (str): Name of the world. 16 | height (float): Height of the world. 17 | width (float): Width of the world. 18 | step_time (float): Time interval between steps. 19 | sample_time (float): Time interval between samples. 20 | offset (list): Offset for the world's position. 21 | control_mode (str): Control mode ('auto' or 'keyboard'). 22 | collision_mode (str): Collision mode ('stop', 'reactive', 'unobstructed'). 23 | obstacle_map: Image file for the obstacle map. 24 | mdownsample (int): Downsampling factor for the obstacle map. 25 | """ 26 | 27 | def __init__( 28 | self, 29 | name: Optional[str] = "world", 30 | height: float = 10, 31 | width: float = 10, 32 | step_time: float = 0.1, 33 | sample_time: float = 0.1, 34 | offset: list = [0, 0], 35 | control_mode: str = "auto", 36 | collision_mode: str = "stop", 37 | obstacle_map=None, 38 | mdownsample: int = 1, 39 | **kwargs, 40 | ) -> None: 41 | """ 42 | Initialize the world object. 43 | 44 | Parameters: 45 | name (str): Name of the world. 46 | height (float): Height of the world. 47 | width (float): Width of the world. 48 | step_time (float): Time interval between steps. 49 | sample_time (float): Time interval between samples. 50 | offset (list): Offset for the world's position. 51 | control_mode (str): Control mode ('auto' or 'keyboard'). 52 | collision_mode (str): Collision mode ('stop', 'reactive', 'unobstructed'). 53 | obstacle_map: Image file for the obstacle map. 54 | mdownsample (int): Downsampling factor for the obstacle map. 55 | """ 56 | 57 | self.name = os.path.basename(name).split(".")[0] 58 | self.height = height 59 | self.width = width 60 | self.step_time = step_time 61 | self.sample_time = sample_time 62 | self.offset = offset 63 | 64 | self.count = 0 65 | self.sampling = True 66 | 67 | self.x_range = [self.offset[0], self.offset[0] + self.width] 68 | self.y_range = [self.offset[1], self.offset[1] + self.height] 69 | 70 | self.grid_map, self.obstacle_index, self.obstacle_positions = self.gen_grid_map( 71 | obstacle_map, mdownsample 72 | ) 73 | 74 | self.plot_parse = kwargs.get("plot", dict()) 75 | 76 | # Set world parameters 77 | world_param.step_time = step_time 78 | world_param.control_mode = control_mode 79 | world_param.collision_mode = collision_mode 80 | 81 | def step(self): 82 | """ 83 | Advance the simulation by one step. 84 | """ 85 | self.count += 1 86 | self.sampling = self.count % (self.sample_time / self.step_time) == 0 87 | 88 | world_param.time = self.time 89 | world_param.count = self.count 90 | 91 | def gen_grid_map(self, obstacle_map, mdownsample=1): 92 | """ 93 | Generate a grid map for obstacles. 94 | 95 | Args: 96 | obstacle_map: Path to the obstacle map image. 97 | mdownsample (int): Downsampling factor. 98 | 99 | Returns: 100 | tuple: Grid map, obstacle indices, and positions. 101 | """ 102 | abs_obstacle_map = file_check(obstacle_map, root_path=pm.root_path + "/world/map") 103 | 104 | if abs_obstacle_map is not None: 105 | grid_map = mpimg.imread(abs_obstacle_map) 106 | grid_map = grid_map[::mdownsample, ::mdownsample] 107 | 108 | if len(grid_map.shape) > 2: 109 | print("convert to grayscale") 110 | grid_map = self.rgb2gray(grid_map) 111 | 112 | grid_map = 100 * (1 - grid_map) # range: 0 - 100 113 | grid_map = np.fliplr(grid_map.T) 114 | 115 | x_reso = self.width / grid_map.shape[0] 116 | y_reso = self.height / grid_map.shape[1] 117 | self.reso = np.array([[x_reso], [y_reso]]) 118 | 119 | obstacle_index = np.array(np.where(grid_map > 50)) 120 | obstacle_positions = obstacle_index * self.reso 121 | 122 | else: 123 | grid_map = None 124 | obstacle_index = None 125 | obstacle_positions = None 126 | self.reso = np.zeros((2, 1)) 127 | 128 | return grid_map, obstacle_index, obstacle_positions 129 | 130 | 131 | def get_map(self, resolution: float = 0.1, obstacle_list: list = []): 132 | """ 133 | Get the map of the world with the given resolution. 134 | """ 135 | return Map(self.width, self.height, resolution, obstacle_list, self.grid_map) 136 | 137 | 138 | def reset(self): 139 | """ 140 | Reset the world simulation. 141 | """ 142 | 143 | world_param.count = 0 144 | self.count = 0 145 | 146 | @property 147 | def time(self): 148 | """ 149 | Get the current simulation time. 150 | 151 | Returns: 152 | float: Current time based on steps and step_time. 153 | """ 154 | return self.count * self.step_time 155 | 156 | @property 157 | def buffer_reso(self): 158 | """ 159 | Get the maximum resolution of the world. 160 | 161 | Returns: 162 | float: Maximum resolution. 163 | """ 164 | return np.max(self.reso) 165 | 166 | def rgb2gray(self, rgb): 167 | return np.dot(rgb[..., :3], [0.2989, 0.5870, 0.1140]) 168 | -------------------------------------------------------------------------------- /irsim/world/world3d.py: -------------------------------------------------------------------------------- 1 | from irsim.world import World 2 | 3 | 4 | class World3D(World): 5 | 6 | def __init__(self, name, depth: float = 10.0, offset=[0, 0, 0], **kwargs): 7 | super().__init__(name=name, **kwargs) 8 | 9 | self.depth = depth 10 | 11 | self.offset = offset if len(offset) == 3 else [offset[0], offset[1], 0] 12 | 13 | self.z_range = [self.offset[2], self.offset[2] + self.depth] 14 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools>=61.0", 3 | ] 4 | 5 | build-backend = "setuptools.build_meta" 6 | 7 | [project] 8 | name = "ir-sim" 9 | version = "2.5.2" 10 | authors = [ 11 | { name="Han Ruihua", email="hanrh@connect.hku.hk" }, 12 | ] 13 | description = "IR-SIM is an open-source, lightweight robot simulator based on Python, designed for robotics navigation, control, and learning. This simulator provides a simple and user-friendly framework for simulating robots, sensors, and environments, facilitating the development and testing of robotics algorithms with minimal hardware requirements." 14 | readme = "README.md" 15 | requires-python = ">=3.9" 16 | classifiers = [ 17 | "Programming Language :: Python :: 3.9", 18 | "Programming Language :: Python :: 3.10", 19 | "Programming Language :: Python :: 3.11", 20 | "Programming Language :: Python :: 3.12", 21 | "Programming Language :: Python :: 3.13", 22 | "License :: OSI Approved :: MIT License", 23 | "Operating System :: OS Independent", 24 | ] 25 | dependencies = [ 26 | 'matplotlib', 27 | 'shapely>=2.0.3', 28 | 'numpy', 29 | 'pyyaml', 30 | 'imageio', 31 | 'loguru', 32 | 'tabulate', 33 | 'scipy', 34 | ] 35 | 36 | [project.optional-dependencies] 37 | keyboard = ['pynput'] 38 | test = ['pytest', 'pytest-cov'] 39 | all = ['pynput'] 40 | 41 | 42 | [tool.setuptools.packages.find] 43 | include = ["irsim*"] 44 | exclude = ["doc*", "tests*"] 45 | 46 | [tool.setuptools.package-data] 47 | "*" = ["*.png", "*.yaml"] 48 | 49 | [project.urls] 50 | "Homepage" = "https://github.com/hanruihua/ir-sim" 51 | "Documentation" = "https://ir-sim.readthedocs.io/en/latest/" 52 | -------------------------------------------------------------------------------- /requirements/requirements_py310.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/requirements/requirements_py310.txt -------------------------------------------------------------------------------- /requirements/requirements_py311.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/requirements/requirements_py311.txt -------------------------------------------------------------------------------- /requirements/requirements_py312.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/requirements/requirements_py312.txt -------------------------------------------------------------------------------- /requirements/requirements_py313.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/requirements/requirements_py313.txt -------------------------------------------------------------------------------- /requirements/requirements_py39.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/requirements/requirements_py39.txt -------------------------------------------------------------------------------- /tests/cave.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hanruihua/ir-sim/67080dae6ee2b3c9f6ad247fd70ff580a4903113/tests/cave.png -------------------------------------------------------------------------------- /tests/custom_behavior.py: -------------------------------------------------------------------------------- 1 | import irsim 2 | 3 | ''' 4 | - Refer to the irsim/lib/behavior_methods.py file for the custom behavior design. 5 | - custom_behavior_methods.py is the designated module name. It should be placed in the same directory as the current implementation script. 6 | - The behavior names defined in custom_behavior_methods.py (e.g., dash_custom) must match the behavior names specified in the YAML file. 7 | ''' 8 | 9 | env = irsim.make() 10 | env.load_behavior("custom_behavior_methods") 11 | 12 | for i in range(1000): 13 | 14 | env.step() 15 | env.render(0.01) 16 | 17 | if env.done(): 18 | break 19 | 20 | env.end(5) 21 | -------------------------------------------------------------------------------- /tests/custom_behavior.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | 11 | robot: 12 | - number: 3 13 | distribution: {name: 'circle', radius: 4.0, center: [5, 5]} 14 | kinematics: {name: 'diff'} 15 | shape: 16 | - {name: 'circle', radius: 0.2} # radius 17 | behavior: {name: 'rvo', vxmax: 1.0, vymax: 1.0, acce: 1.0, factor: 1.0} 18 | vel_min: [-3, -3.0] 19 | vel_max: [3, 3.0] 20 | color: ['royalblue', 'red', 'green'] 21 | arrive_mode: position 22 | goal_threshold: 0.2 23 | plot: 24 | show_trail: true 25 | show_goal: true 26 | trail_fill: true 27 | trail_alpha: 0.2 28 | show_trajectory: false 29 | 30 | - number: 2 31 | distribution: {name: 'circle', radius: 4.0, center: [5, 5]} 32 | kinematics: {name: 'diff'} 33 | shape: 34 | - {name: 'circle', radius: 0.2} # radius 35 | behavior: {name: 'dash', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 1.0} 36 | vel_min: [-3, -3.0] 37 | vel_max: [3, 3.0] 38 | color: ['orange', 'purple'] 39 | arrive_mode: position 40 | goal_threshold: 0.2 41 | plot: 42 | show_trail: true 43 | show_goal: true 44 | trail_fill: true 45 | trail_alpha: 0.2 46 | show_trajectory: false 47 | 48 | - number: 2 49 | distribution: {name: 'random'} 50 | kinematics: {name: 'diff'} 51 | shape: 52 | - {name: 'circle', radius: 0.2} # radius 53 | behavior: {name: 'dash_custom'} 54 | vel_min: [-3, -3.0] 55 | vel_max: [3, 3.0] 56 | color: ['pink', 'brown'] 57 | arrive_mode: position 58 | goal_threshold: 0.2 59 | plot: 60 | show_trail: true 61 | show_goal: true 62 | trail_fill: true 63 | trail_alpha: 0.2 64 | show_trajectory: false 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /tests/custom_behavior_methods.py: -------------------------------------------------------------------------------- 1 | from irsim.lib import register_behavior 2 | from irsim.util.util import relative_position, WrapToPi 3 | import numpy as np 4 | 5 | 6 | @register_behavior("diff", "dash_custom") 7 | def beh_diff_dash(ego_object, external_objects=[], **kwargs): 8 | 9 | print("This is a custom behavior example for differential drive with dash2") 10 | 11 | state = ego_object.state 12 | goal = ego_object.goal 13 | goal_threshold = ego_object.goal_threshold 14 | _, max_vel = ego_object.get_vel_range() 15 | angle_tolerance = kwargs.get("angle_tolerance", 0.1) 16 | 17 | behavior_vel = DiffDash2(state, goal, max_vel, goal_threshold, angle_tolerance) 18 | 19 | return behavior_vel 20 | 21 | 22 | def DiffDash2(state, goal, max_vel, goal_threshold=0.1, angle_tolerance=0.2): 23 | """ 24 | Calculate the differential drive velocity to reach a goal. 25 | 26 | Args: 27 | state (np.array): Current state [x, y, theta] (3x1). 28 | goal (np.array): Goal position [x, y, theta] (3x1). 29 | max_vel (np.array): Maximum velocity [linear, angular] (2x1). 30 | goal_threshold (float): Distance threshold to consider goal reached (default 0.1). 31 | angle_tolerance (float): Allowable angular deviation (default 0.2). 32 | 33 | Returns: 34 | np.array: Velocity [linear, angular] (2x1). 35 | """ 36 | distance, radian = relative_position(state, goal) 37 | 38 | if distance < goal_threshold: 39 | return np.zeros((2, 1)) 40 | 41 | diff_radian = WrapToPi(radian - state[2, 0]) 42 | linear = max_vel[0, 0] * np.cos(diff_radian) 43 | 44 | if abs(diff_radian) < angle_tolerance: 45 | angular = 0 46 | else: 47 | angular = max_vel[1, 0] * np.sign(diff_radian) 48 | 49 | return np.array([[linear], [angular]]) -------------------------------------------------------------------------------- /tests/test.md: -------------------------------------------------------------------------------- 1 | pytest --cov=ir-sim --cov-report html -------------------------------------------------------------------------------- /tests/test_all_objects.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | 9 | robot: 10 | - kinematics: {name: 'diff', noise: True} # omni, diff, ackers 11 | shape: {name: 'circle', radius: 0.2} # radius 12 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 13 | state: [1, 1, 0] 14 | goal: [9, 9, 0] 15 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 16 | behavior: {name: 'dash'} # move toward to the goal directly 17 | 18 | sensors: 19 | - type: 'lidar2d' 20 | range_min: 0 21 | range_max: 10 22 | angle_range: 3.1415926 23 | number: 100 24 | noise: False 25 | has_velocity: True 26 | 27 | - kinematics: {name: 'diff', noise: True} # omni, diff, ackers 28 | shape: {name: 'circle', radius: 0.2} # radius 29 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 30 | state: [1, 1, 0] 31 | goal: [9, 9, 0] 32 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 33 | sensors: 34 | - type: 'lidar2d' 35 | range_min: 0 36 | range_max: 10 37 | angle_range: 3.1415926 38 | number: 100 39 | noise: False 40 | has_velocity: True 41 | 42 | - kinematics: {name: 'acker'} # omni, diff, acker 43 | shape: {name: 'circle', radius: 0.2, wheelbase: 0.2} # radius 44 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 45 | state: [5, 1, 0, 0] 46 | goal: [9, 9, 0] 47 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 48 | behavior: {name: 'dash'} # move toward to the goal directly 49 | plot: 50 | show_goal: True 51 | 52 | obstacle: 53 | - kinematics: {name: 'omni'} 54 | behavior: {name: 'dash'} 55 | shape: {name: 'circle', radius: 1.0} # radius 56 | state: [5, 5, 0] 57 | plot: 58 | obj_linestyle: '--' 59 | 60 | - shape: {name: 'rectangle', length: 1.5, width: 1.2} # radius 61 | state: [6, 5, 1] 62 | behavior: {name: 'dash', range_low: [0, 0, -3.14], range_high: [10, 10, 3.14], wander: True} 63 | 64 | - shape: {name: 'polygon', vertices: [[6, 5], [7, 5], [7, 6], [6, 6]]} # radius 65 | state: [6, 5, 1] 66 | behavior: {name: 'dash', range_low: [0, 0, -3.14], range_high: [10, 10, 3.14], wander: True} 67 | 68 | - shape: {name: 'linestring', vertices: [[5, 5], [4, 0], [1, 6]] } # vertices 69 | state: [0, 0, 0] 70 | unobstructed: True 71 | plot: 72 | obj_linestyle: '-.' 73 | 74 | - number: 5 75 | distribution: {name: 'random', range_low: [0, 0, -3.14], range_high: [10, 10, 3.14]} 76 | kinematics: {name: 'diff'} 77 | shape: 78 | - {name: 'circle', radius: 0.1} # radius 79 | - {name: 'circle', radius: 0.2} # radius 80 | 81 | behavior: 82 | - {name: 'dash', range_low: [0, 0, -3.14], range_high: [10, 10, 3.14], wander: True} 83 | 84 | vel_min: [-0.5, -3.14] 85 | vel_max: [0.5, 3.14] 86 | arrive_mode: position 87 | goal_threshold: 0.3 88 | plot: 89 | show_goal: False 90 | show_arrow: True 91 | -------------------------------------------------------------------------------- /tests/test_all_objects_3d.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import irsim 3 | from irsim.util.util import time_it 4 | from unittest.mock import Mock, patch 5 | from pynput import keyboard 6 | import matplotlib.pyplot as plt 7 | import time 8 | 9 | @pytest.fixture(autouse=True) 10 | def setup_teardown(): 11 | """Setup and cleanup before and after each test""" 12 | plt.close('all') 13 | yield 14 | plt.close('all') 15 | 16 | def test_collision_avoidance_3d(): 17 | """Test collision avoidance functionality in 3D projection""" 18 | env = irsim.make("test_collision_avoidance.yaml", save_ani=False, full=False, display=False, projection='3d') 19 | 20 | for i in range(50): 21 | env.step() 22 | env.render(0.01) 23 | env.draw_trajectory(env.robot.trajectory, show_direction=True) 24 | if env.done(): 25 | break 26 | 27 | env.end() 28 | assert True # Add specific assertions 29 | 30 | def test_polygon_and_lidar_3d(): 31 | """Test polygon shape and lidar functionality in 3D projection""" 32 | env = irsim.make('test_all_objects.yaml', display=False, projection='3d') 33 | 34 | env.random_polygon_shape() 35 | points = env.robot.get_lidar_points() 36 | env.draw_points(points) 37 | 38 | scan = env.robot.get_lidar_scan() 39 | offset = env.robot.get_lidar_offset() 40 | gh_init = env.robot.get_init_Gh() 41 | gh = env.robot.get_Gh() 42 | 43 | env.get_obstacle_info_list() 44 | env.get_robot_info_list() 45 | env.delete_objects([1, 2]) 46 | 47 | for i in range(10): 48 | env.step() 49 | env.render(0.01) 50 | env.end() 51 | 52 | # assert points is not None 53 | assert scan is not None 54 | assert offset is not None 55 | 56 | def test_animation_saving_3d(): 57 | """Test animation saving functionality in 3D projection""" 58 | env = irsim.make('test_render.yaml', save_ani=True, display=False, projection='3d') 59 | 60 | for i in range(3): 61 | env.step() 62 | env.render(0.01) 63 | env.end(ani_name='test_animation') 64 | assert True # Add file existence check 65 | 66 | def test_collision_world_3d(): 67 | """Test collision world in 3D projection""" 68 | env = irsim.make('test_collision_world.yaml', save_ani=False, display=False, projection='3d') 69 | 70 | for i in range(4): 71 | env.step() 72 | env.render(0.01) 73 | env.end() 74 | assert True # Add collision detection assertions 75 | 76 | def test_multi_objects_3d(): 77 | """Test multi-object scenario in 3D projection""" 78 | env = irsim.make('test_multi_objects_world.yaml', save_ani=False, display=False, projection='3d') 79 | env.robot.set_goal([5, 10, 0]) 80 | env.random_obstacle_position() 81 | 82 | for i in range(5): 83 | env.step() 84 | env.render(0.01) 85 | env.end() 86 | assert True # Add multi-object related assertions 87 | 88 | def test_grid_map_3d(): 89 | """Test grid map in 3D projection""" 90 | env = irsim.make('test_grid_map.yaml', save_ani=False, display=False, projection='3d') 91 | 92 | for i in range(6): 93 | env.step() 94 | env.render(0.01) 95 | 96 | gh = env.robot.get_init_Gh() 97 | env.end() 98 | assert gh is not None 99 | 100 | def test_keyboard_control_3d(): 101 | """Test keyboard control in 3D projection""" 102 | env = irsim.make('test_keyboard_control.yaml', save_ani=False, display=False, projection='3d') 103 | key_list = ['w', 'a', 's', 'd', 'q', 'e', 'z', 'c', 'r'] 104 | mock_keys = [Mock(spec=keyboard.Key, char=c) for c in key_list] 105 | 106 | for i in range(3): 107 | for mock_key in mock_keys: 108 | env._on_press(mock_key) 109 | env._on_release(mock_key) 110 | env.step() 111 | env.render(0.01) 112 | env.end() 113 | assert True # Add keyboard control related assertions 114 | 115 | def test_custom_behavior_3d(): 116 | """Test custom behavior in 3D projection""" 117 | env = irsim.make('custom_behavior.yaml', display=False, projection='3d') 118 | env.load_behavior("custom_behavior_methods") 119 | 120 | for i in range(10): 121 | env.step() 122 | env.render(0.01) 123 | env.end() 124 | assert True # Add behavior related assertions 125 | 126 | def test_fov_detection_3d(): 127 | """Test field of view detection in 3D projection""" 128 | env = irsim.make('test_fov_world.yaml', save_ani=False, display=False, projection='3d') 129 | 130 | for i in range(10): 131 | detected = [obs.fov_detect_object(env.robot) for obs in env.obstacle_list] 132 | env.step() 133 | env.render(0.01) 134 | env.end() 135 | assert isinstance(detected, list) 136 | 137 | def test_3d_projection(): 138 | """Test 3D projection specifically""" 139 | env = irsim.make('test_multi_objects_world.yaml', save_ani=False, display=False, projection='3d') 140 | env.random_obstacle_position(ids=[3, 4, 5, 6, 7], non_overlapping=True) 141 | 142 | for i in range(5): 143 | env.step() 144 | env.render(0.01) 145 | env.end() 146 | assert True # Add 3D related assertions 147 | 148 | if __name__ == "__main__": 149 | pytest.main(["--cov=.", "--cov-report", "html", "-v", __file__]) 150 | -------------------------------------------------------------------------------- /tests/test_collision_avoidance.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'unobstructed' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | plot: 10 | no_axis: true 11 | 12 | robot: 13 | - number: 2 14 | distribution: {name: 'circle', radius: 2.0, center: [5, 5]} 15 | kinematics: 16 | - {name: 'omni'} 17 | - {name: 'diff'} 18 | 19 | shape: 20 | - {name: 'circle', radius: 0.2} # radius 21 | behavior: {name: 'rvo', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 1.0, mode: 'rvo'} 22 | vel_min: [-3, -3.0] 23 | vel_max: [3, 3.0] 24 | color: ['royalblue', 'red', 'green', 'orange', 'purple', 'yellow', 'cyan', 'magenta', 'lime', 'pink', 'brown'] 25 | arrive_mode: state 26 | goal_threshold: 0.15 27 | 28 | sensors: 29 | - type: 'lidar2d' 30 | range_min: 0 31 | range_max: 20 32 | angle_range: 3.14 33 | number: 100 34 | noise: False 35 | std: 1 36 | angle_std: 0.2 37 | offset: [0, 0, 0] 38 | reso: 0.1 39 | alpha: 0.4 40 | 41 | plot: 42 | show_trail: true 43 | show_goal: true 44 | show_fov: true 45 | show_text: true 46 | show_arrow: true 47 | show_trajectory: true 48 | trail_fill: true 49 | trail_alpha: 0.2 50 | 51 | - number: 10 52 | distribution: {name: 'circle', radius: 3.0, center: [5, 5]} 53 | kinematics: {name: 'diff'} 54 | shape: 55 | - {name: 'circle', radius: 0.2} # radius 56 | behavior: {name: 'rvo', vxmax: 3.5, vymax: 3.5, acce: 1.0, factor: 1.0, mode: 'vo'} 57 | vel_min: [-3, -3.0] 58 | vel_max: [3, 3.0] 59 | color: ['royalblue', 'red', 'green', 'orange', 'purple', 'yellow', 'cyan', 'magenta', 'lime', 'pink', 'brown'] 60 | arrive_mode: position 61 | goal_threshold: 0.15 62 | plot: 63 | show_trail: true 64 | show_goal: true 65 | show_text: true 66 | show_arrow: true 67 | show_trajectory: true 68 | trail_fill: true 69 | trail_alpha: 0.2 70 | 71 | 72 | - number: 2 73 | distribution: {name: 'circle', radius: 4.0, center: [5, 5]} 74 | kinematics: {name: 'diff'} 75 | shape: 76 | - {name: 'circle', radius: 0.2} # radius 77 | behavior: {name: 'rvo', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 1.0, mode: 'hrvo'} 78 | vel_min: [-3, -3.0] 79 | vel_max: [3, 3.0] 80 | color: ['royalblue', 'red', 'green', 'orange', 'purple', 'yellow', 'cyan', 'magenta', 'lime', 'pink', 'brown'] 81 | arrive_mode: position 82 | goal_threshold: 0.15 83 | plot: 84 | show_trail: true 85 | show_goal: true 86 | trail_fill: true 87 | trail_alpha: 0.2 88 | show_trajectory: false 89 | 90 | obstacle: 91 | - number: 2 92 | distribution: {name: 'circle', radius: 2.0, center: [5, 5]} 93 | kinematics: {name: 'omni'} 94 | behavior: {name: 'rvo'} 95 | shape: {name: 'circle', radius: 1.0} # radius 96 | state: [5, 5] 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /tests/test_collision_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'unobstructed_obstacles' # 'stop', 'unobstructed', 'reactive', 'unobstructed_obstacles' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'diff'} # omni, diff, acker 12 | shape: {name: 'circle', radius: 0.2} # radius 13 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 14 | state: [1, 1, 0] 15 | goal: [9, 9, 0] 16 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 17 | behavior: {name: 'dash'} # move toward to the goal directly 18 | 19 | obstacle: 20 | - shape: {name: 'circle', radius: 1.0} # radius 21 | state: [5, 5, 0] 22 | 23 | - shape: {name: 'rectangle', length: 1.5, width: 1.2} # radius 24 | state: [6, 5, 1] 25 | 26 | - number: 2 27 | distribution: {name: 'circle', radius: 2.0, center: [5, 5]} 28 | kinematics: {name: 'omni'} 29 | behavior: {name: 'rvo'} 30 | shape: {name: 'circle', radius: 1.0} # radius 31 | state: [5, 5, 0] 32 | 33 | 34 | # - shape: {name: 'circle', radius: 0.4} # radius 35 | # state: [1, 1, 0] -------------------------------------------------------------------------------- /tests/test_fov_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | # control_mode: 'keyboard' # 'keyboard', 'auto' 10 | 11 | robot: 12 | - kinematics: {name: 'diff'} # omni, diff, acker 13 | shape: {name: 'circle', radius: 0.4} 14 | state: [5, 5, 0, 0] 15 | goal: [40, 40, 0] 16 | vel_max: [3, 1] 17 | vel_min: [-3, -1] 18 | behavior: {name: 'dash'} # move toward to the goal directly 19 | plot: 20 | show_goal: False 21 | show_trajectory: True 22 | 23 | obstacle: 24 | - number: 10 25 | distribution: {name: 'random', range_low: [10, 10, -3.14], range_high: [40, 40, 3.14]} 26 | kinematics: {name: 'diff'} 27 | behavior: {name: 'rvo', vxmax: 1.5, vymax: 1.5, acce: 1.0, factor: 2.0, mode: 'vo', wander: True, range_low: [15, 15, -3.14], range_high: [35, 35, 3.14], target_roles: 'all'} 28 | vel_max: [3, 3.14] 29 | vel_min: [-3, -3.14] 30 | shape: 31 | - {name: 'circle', radius: 0.5} # radius 32 | fov: 1.57 33 | fov_radius: 5.0 34 | plot: 35 | show_fov: True 36 | show_arrow: True 37 | arrow_length: 0.8 38 | -------------------------------------------------------------------------------- /tests/test_grid_map.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | obstacle_map: 'cave.png' 10 | mdownsample: 2 11 | 12 | robot: 13 | - kinematics: {name: 'acker'} # omni, diff, acker 14 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 15 | state: [5, 5, 0, 0] 16 | goal: [40, 40, 0] 17 | vel_max: [4, 1] 18 | behavior: {name: 'dash'} # move toward to the goal directly 19 | plot: 20 | show_trail: True 21 | traj_color: 'g' 22 | show_trajectory: True 23 | show_goal: False 24 | trail_type: 'polygon' 25 | 26 | sensors: 27 | - type: 'lidar2d' 28 | range_min: 0 29 | range_max: 20 30 | angle_range: 3.14 31 | number: 100 32 | noise: False 33 | std: 1 34 | angle_std: 0.2 35 | offset: [0, 0, 0] 36 | reso: 0.1 37 | alpha: 0.4 38 | 39 | plot: 40 | show_trajectory: True 41 | 42 | 43 | obstacle: 44 | - number: 10 45 | distribution: {name: 'manual'} 46 | shape: 47 | - {name: 'polygon', random_shape: true, center_range: [5, 10, 40, 30], avg_radius_range: [0.5, 2]} # radius 48 | 49 | -------------------------------------------------------------------------------- /tests/test_keyboard_control.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 50 # the height of the world 3 | width: 50 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'keyboard' # 'keyboard', 'auto' 9 | 10 | robot: 11 | - kinematics: {name: 'acker'} # omni, diff, acker 12 | shape: {name: 'rectangle', length: 4.6, width: 1.6, wheelbase: 3} 13 | state: [5, 5, 0, 0] 14 | goal: [40, 40, 0] 15 | vel_max: [4, 1] 16 | behavior: {name: 'dash'} # move toward to the goal directly 17 | plot: 18 | show_trail: True 19 | traj_color: 'g' 20 | 21 | sensors: 22 | - type: 'lidar2d' 23 | range_min: 0 24 | range_max: 20 25 | angle_range: 3.14 26 | number: 100 27 | noise: False 28 | std: 1 29 | angle_std: 0.2 30 | offset: [0, 0, 0] 31 | reso: 0.1 32 | alpha: 0.4 33 | 34 | 35 | obstacle: 36 | - number: 10 37 | distribution: {name: 'manual'} 38 | shape: 39 | - {name: 'polygon', random_shape: true, center_range: [5, 10, 40, 30], avg_radius_range: [0.5, 2]} # radius 40 | 41 | -------------------------------------------------------------------------------- /tests/test_kinematics.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import numpy as np 3 | from irsim.lib.algorithm.kinematics import ( 4 | differential_kinematics, 5 | ackermann_kinematics, 6 | omni_kinematics 7 | ) 8 | 9 | def test_differential_kinematics(): 10 | """Test differential drive robot kinematics""" 11 | # Test basic movement 12 | state = np.array([[0], [0], [0]]) # x, y, theta 13 | velocity = np.array([[1], [0]]) # linear, angular 14 | next_state = differential_kinematics(state, velocity, 1.0) 15 | assert np.allclose(next_state, np.array([[1], [0], [0]])) 16 | 17 | # Test rotation 18 | velocity = np.array([[0], [1]]) # linear, angular 19 | next_state = differential_kinematics(state, velocity, 1.0) 20 | assert np.allclose(next_state, np.array([[0], [0], [1]])) 21 | 22 | # Test with noise 23 | next_state_noisy = differential_kinematics(state, velocity, 1.0, noise=True) 24 | assert next_state_noisy.shape == (3, 1) 25 | 26 | # Test angle wrapping 27 | state = np.array([[0], [0], [np.pi]]) 28 | velocity = np.array([[0], [np.pi]]) 29 | next_state = differential_kinematics(state, velocity, 1.0) 30 | assert np.allclose(next_state[2], 0) 31 | 32 | def test_ackermann_kinematics(): 33 | """Test Ackermann steering vehicle kinematics""" 34 | # Test steer mode 35 | state = np.array([[0], [0], [0], [0]]) # x, y, theta, steer_angle 36 | velocity = np.array([[1], [0]]) # linear, steer_angle 37 | next_state = ackermann_kinematics(state, velocity, 1.0, mode="steer") 38 | assert np.allclose(next_state[:3], np.array([[1], [0], [0]])) 39 | 40 | # Test angular mode 41 | velocity = np.array([[1], [0.1]]) # linear, angular 42 | next_state = ackermann_kinematics(state, velocity, 1.0, mode="angular") 43 | assert next_state.shape == (4, 1) 44 | 45 | # Test with noise 46 | next_state_noisy = ackermann_kinematics(state, velocity, 1.0, noise=True) 47 | assert next_state_noisy.shape == (4, 1) 48 | 49 | # Test angle wrapping 50 | state = np.array([[0], [0], [np.pi], [0]]) 51 | velocity = np.array([[0], [np.pi]]) 52 | next_state = ackermann_kinematics(state, velocity, 1.0) 53 | assert np.allclose(next_state[2], np.pi) 54 | 55 | def test_omni_kinematics(): 56 | """Test omnidirectional robot kinematics""" 57 | # Test basic movement 58 | state = np.array([[0], [0]]) # x, y 59 | velocity = np.array([[1], [0]]) # vx, vy 60 | next_state = omni_kinematics(state, velocity, 1.0) 61 | assert np.allclose(next_state, np.array([[1], [0]])) 62 | 63 | # Test diagonal movement 64 | velocity = np.array([[1], [1]]) # vx, vy 65 | next_state = omni_kinematics(state, velocity, 1.0) 66 | assert np.allclose(next_state, np.array([[1], [1]])) 67 | 68 | # Test with noise 69 | next_state_noisy = omni_kinematics(state, velocity, 1.0, noise=True) 70 | assert next_state_noisy.shape == (2, 1) 71 | 72 | def test_kinematics_error_handling(): 73 | """Test error handling in kinematics functions""" 74 | # Test invalid state dimensions 75 | with pytest.raises(AssertionError): 76 | state = np.array([[0], [0]]) # Too few dimensions 77 | velocity = np.array([[1], [0]]) 78 | differential_kinematics(state, velocity, 1.0) 79 | 80 | # Test invalid velocity dimensions 81 | with pytest.raises(AssertionError): 82 | state = np.array([[0], [0], [0]]) 83 | velocity = np.array([[1]]) # Too few dimensions 84 | differential_kinematics(state, velocity, 1.0) 85 | 86 | # Test invalid noise parameters 87 | with pytest.raises(AssertionError): 88 | state = np.array([[0], [0], [0]]) 89 | velocity = np.array([[1], [0]]) 90 | differential_kinematics(state, velocity, 1.0, noise=True, alpha=[0.03]) # Too few parameters 91 | 92 | if __name__ == "__main__": 93 | test_differential_kinematics() 94 | test_ackermann_kinematics() 95 | test_omni_kinematics() 96 | test_kinematics_error_handling() -------------------------------------------------------------------------------- /tests/test_multi_objects_world.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'stop' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | 11 | robot: 12 | - number: 2 13 | distribution: {name: 'manual'} 14 | kinematics: {name: 'diff'} 15 | shape: 16 | - {name: 'circle', radius: 0.2} # radius 17 | state: 18 | - [1, 1, 0] 19 | - [2, 1, 0] 20 | goal: 21 | - [9, 9, 0] 22 | - [9, 2, 0] 23 | behavior: 24 | - {name: 'dash'} # move toward to the goal directly 25 | - {name: 'dash'} # move toward to the goal directly 26 | color: 27 | - 'royalblue' 28 | - 'red' 29 | 30 | - number: 4 31 | distribution: {name: 'random'} 32 | kinematics: {name: 'diff'} 33 | shape: 34 | - {name: 'circle', radius: 0.2} # radius 35 | - {name: 'rectangle', length: 1.5, width: 1.2} # radiu 36 | behavior: 37 | - {name: 'dash'} # move toward to the goal directly 38 | color: 39 | - 'pink' 40 | 41 | plot: 42 | goal_color: 'purple' 43 | 44 | 45 | obstacle: 46 | - number: 10 47 | distribution: {name: 'manual'} 48 | state: [[4, 8], [31, 38], [10, 20], [41, 25], [20, 13], [16, 26], [10, 24], [18, 20], [16, 26], [19, 26], [10, 30]] 49 | shape: 50 | - {name: 'circle', radius: 0.4} # radius 51 | - {name: 'circle', radius: 0.1} # radius 52 | color: 'k' 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /tests/test_path_planners.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | 3 | import irsim 4 | from irsim.lib.path_planners.a_star import AStarPlanner 5 | from irsim.lib.path_planners.probabilistic_road_map import PRMPlanner 6 | from irsim.lib.path_planners.rrt import RRT 7 | from irsim.lib.path_planners.rrt_star import RRTStar 8 | 9 | 10 | @pytest.mark.parametrize( 11 | "planner, resolution", 12 | [ 13 | (AStarPlanner, 0.3), 14 | (RRTStar, 0.3), 15 | (RRT, 0.3), 16 | (PRMPlanner, 0.3), 17 | ], 18 | ) 19 | def test_path_planners(planner, resolution): 20 | env = irsim.make( 21 | "test_collision_world.yaml", save_ani=False, full=False, display=False 22 | ) 23 | env_map = env.get_map() 24 | planner = planner(env_map, resolution) 25 | robot_info = env.get_robot_info() 26 | robot_state = env.get_robot_state() 27 | trajectory = planner.planning(robot_state, robot_info.goal) 28 | env.draw_trajectory(trajectory, traj_type="r-") 29 | -------------------------------------------------------------------------------- /tests/test_random_goals.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | import numpy as np 4 | import shapely 5 | from irsim.lib.handler.geometry_handler import GeometryFactory 6 | from irsim.util.util import time_it 7 | import irsim 8 | import matplotlib.pyplot as plt 9 | 10 | def check_goal(goal, obstacle_list, goal_check_radius): 11 | shape = {"name": "circle", "radius": goal_check_radius} 12 | gf = GeometryFactory.create_geometry(**shape) 13 | geometry = gf.step(np.c_[goal]) 14 | covered_goal = any( 15 | [ 16 | shapely.intersects(geometry, obj._geometry) 17 | for obj in obstacle_list 18 | ] 19 | ) 20 | 21 | return covered_goal 22 | 23 | 24 | @time_it("test_all_objects") 25 | def test_random_goals(): 26 | goal_check_radius = 0.4 27 | env = irsim.make('test_collision_world.yaml', save_ani=False, display=False) 28 | env.robot.set_goal([5, 10, 0], init=True) 29 | 30 | env_objects = env.obstacle_list 31 | 32 | goal = env.robot._goal[0] 33 | covered_goal = check_goal(goal, env_objects, goal_check_radius) 34 | assert covered_goal == False 35 | 36 | env.robot.set_goal([5, 5, 0], init=True) 37 | goal = env.robot._goal[0] 38 | covered_goal = check_goal(goal, env_objects, goal_check_radius) 39 | assert covered_goal == True 40 | 41 | for _ in range(100): 42 | env.robot.set_random_goal(env_objects, goal_check_radius=goal_check_radius) 43 | goal = env.robot._goal[0] 44 | covered_goal = check_goal(goal, env_objects, goal_check_radius) 45 | assert covered_goal == False 46 | 47 | env.robot.set_goal([[5, 10, 0],[5, 9, 0],[5, 8, 0]], init=True) 48 | 49 | for _ in range(100): 50 | env.robot.set_random_goal(env_objects, goal_check_radius=goal_check_radius, range_limits=[[3, 3, -3.141592653589793], [7, 7, 3.141592653589793]]) 51 | goals = env.robot._goal 52 | for goal in goals: 53 | covered_goal = check_goal(goal, env_objects, goal_check_radius) 54 | assert covered_goal == False 55 | assert all([3 < point[0] < 7 for point in goals]) 56 | assert all([3 < point[1] < 7 for point in goals]) 57 | goal = env.robot._goal[0] 58 | assert len(goal) == 3 59 | 60 | 61 | if __name__ == "__main__": 62 | test_random_goals() 63 | -------------------------------------------------------------------------------- /tests/test_render.yaml: -------------------------------------------------------------------------------- 1 | world: 2 | height: 10 # the height of the world 3 | width: 10 # the width of the world 4 | step_time: 0.1 # 10Hz calculate each step 5 | sample_time: 0.1 # 10 Hz for render and data extraction 6 | offset: [0, 0] # the offset of the world on x and y 7 | collision_mode: 'unobstructed2' # 'stop', 'unobstructed', 'reactive' 8 | control_mode: 'auto' # 'keyboard', 'auto' 9 | 10 | plot: 11 | no_axis: False 12 | 13 | robot: 14 | - kinematics: {name: 'diff'} # omni, diff, acker 15 | shape: {name: 'circle', radius: 0.2} # radius 16 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 17 | state: [1, 1, 0] 18 | goal: 19 | - [[2, 1, 0], [3, 1, 0]] 20 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 21 | behavior: {name: 'dash'} # move toward to the goal directly 22 | 23 | - kinematics: {name: 'diff'} # omni, diff, acker 24 | shape: {name: 'circle', radius: 0.2} # radius 25 | # shape: {name: 'rectangle', length: 0.5, width: 0.2} # radius 26 | state: [5, 1, 0] 27 | goal: [2, 6, 0] 28 | # acce: [3, .inf] # acce of [linear, angular] or [v_x, v_y] or [linear, steer] 29 | behavior: {name: 'dash'} # move toward to the goal directly 30 | color: 'royalblue' 31 | plot: {show_trajectory: True, show_trail: True, trail_fill: True, trail_alpha: 0.2, show_text: True} 32 | 33 | obstacle: 34 | - number: 10 35 | distribution: {name: 'manual'} 36 | shape: 37 | - {name: 'circle', radius: 1.5} # radius 38 | - {name: 'circle', radius: 1.0} # radius 39 | 40 | state: [[20, 34], [31, 38], [10, 20], [41, 25], [20, 13], [16, 26], [10, 24], [18, 20], [16, 26], [19, 26], [10, 30]] 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | # - shape: {name: 'circle', radius: 0.4} # radius 49 | # state: [1, 1, 0] -------------------------------------------------------------------------------- /tests/test_util.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | import numpy as np 3 | from irsim.util import util 4 | import math 5 | 6 | def test_WrapToPi(): 7 | assert util.WrapToPi(0) == 0 8 | assert util.WrapToPi(math.pi) == math.pi 9 | assert util.WrapToPi(-math.pi) == -math.pi 10 | assert util.WrapToPi(3*math.pi) == math.pi 11 | assert util.WrapToPi(-3*math.pi) == -math.pi 12 | assert util.WrapToPi(math.pi, positive=True) == math.pi 13 | assert util.WrapToPi(-math.pi, positive=True) == math.pi 14 | 15 | def test_WrapToRegion(): 16 | region = [-math.pi, math.pi] 17 | assert util.WrapToRegion(3*math.pi, region) == math.pi 18 | assert util.WrapToRegion(-3*math.pi, region) == -math.pi 19 | assert util.WrapToRegion(0, region) == 0 20 | 21 | def test_convert_list_length(): 22 | assert util.convert_list_length(1, 3) == [1, 1, 1] 23 | assert util.convert_list_length([2], 2) == [[2], [2]] 24 | assert util.convert_list_length([1, 2], 1) == [[1,2]] 25 | assert util.convert_list_length([1, 2], 3) == [[1, 2], [1, 2], [1, 2]] 26 | assert util.convert_list_length([], 0) == [] 27 | 28 | def test_convert_list_length_dict(): 29 | d = {'a': 1} 30 | assert util.convert_list_length_dict(d, 2) == [d, d] 31 | assert util.convert_list_length_dict([d], 2) == [[d], [d]] 32 | assert util.convert_list_length_dict([d, d], 1) == [[d, d]] 33 | assert util.convert_list_length_dict([d, d], 3) == [[d, d], [d, d], [d, d]] 34 | assert util.convert_list_length_dict([], 0) == [] 35 | 36 | def test_is_list_of_dicts(): 37 | assert util.is_list_of_dicts([{'a': 1}, {'b': 2}]) 38 | assert not util.is_list_of_dicts([{'a': 1}, 2]) 39 | assert not util.is_list_of_dicts([1, 2]) 40 | assert not util.is_list_of_dicts('notalist') 41 | 42 | def test_is_list_of_numbers(): 43 | assert util.is_list_of_numbers([1, 2, 3.5]) 44 | assert not util.is_list_of_numbers([1, 'a']) 45 | assert not util.is_list_of_numbers('notalist') 46 | 47 | def test_is_list_of_lists(): 48 | assert util.is_list_of_lists([[1], [2]]) 49 | assert not util.is_list_of_lists([1, 2]) 50 | assert not util.is_list_of_lists('notalist') 51 | 52 | def test_is_list_not_list_of_lists(): 53 | assert util.is_list_not_list_of_lists([1, 2, 3]) 54 | assert not util.is_list_not_list_of_lists([[1], [2]]) 55 | assert not util.is_list_not_list_of_lists('notalist') 56 | 57 | def test_distance(): 58 | assert util.distance(np.array([0, 0]).reshape(2, 1), np.array([0, 0]).reshape(2, 1)) == 0 59 | 60 | def test_random_point_range(): 61 | pt = util.random_point_range([0, 0, -math.pi], [1, 1, math.pi]) 62 | assert len(pt) == 3 63 | assert 0 <= pt[0] <= 1 64 | assert 0 <= pt[1] <= 1 65 | assert -math.pi <= pt[2] <= math.pi 66 | 67 | if __name__ == "__main__": 68 | test_WrapToPi() 69 | test_WrapToRegion() 70 | test_convert_list_length() 71 | test_convert_list_length_dict() 72 | test_is_list_of_dicts() 73 | test_is_list_of_numbers() 74 | test_is_list_of_lists() 75 | test_distance() 76 | test_random_point_range() -------------------------------------------------------------------------------- /todo_list.md: -------------------------------------------------------------------------------- 1 | # IR-SIM Todo List 2 | 3 | ## 🎯 Core Framework 4 | - [x] Basic framework 5 | - [x] Mobile robot movement 6 | - [x] Collision check 7 | - [x] Multi robots mode (collision) 8 | - [x] Reformulate obstacles and robots by Object class 9 | - [x] Construct the object base class for the robot and obstacles 10 | - [x] Rearrange the framework of obstacles 11 | - [x] Add function to construct obstacle and robot 12 | - [x] Attribute of the obstacles and robots 13 | - [x] Formulate the geometry tree for the collision check 14 | - [ ] All the rotation and translation can be represented by the homogeneous transformation matrix 15 | 16 | ## 🤖 Robot & Movement 17 | - [x] Omni directional robots 18 | - [x] Add custom robot model 19 | - [x] Robot description 20 | - [x] Check the dimension of various values and fix the input error, such as state dim, velocity dim 21 | - [x] Reformulate the behavior library 22 | - [ ] Real robot size (LIMO, BYD) 23 | - [ ] Provide polygon shape robot 24 | - [ ] 3D rigid body 25 | - [ ] Robotics arm, UAV support 26 | - [ ] Interface with URDF file 27 | 28 | ## 📡 Sensors & Data 29 | - [x] Sensor lidar 30 | - [x] Add sensor: gps, odometry 31 | - [x] Add noise (diff) 32 | - [ ] Develop Tools for tackling Data. Add the data monitor 33 | - [ ] Record and replay path 34 | - [ ] Analyze the performance (jerks, acceleration, etc.) 35 | 36 | ## 🗺️ Environment & Obstacles 37 | - [x] Line obstacle 38 | - [x] Map obstacle 39 | - [x] Add functions to access obstacles with different types 40 | - [x] Support the feature of adding or eliminating obstacles by functions 41 | - [x] Add binary occupancy grid map for indoor navigation 42 | - [x] Check whether the object is convex 43 | - [ ] Add functions to access obstacles with different types (refinement needed) 44 | - [ ] Develop a lib for configuration of the shape, refer to rviz marker 45 | - [ ] Use scipy convex hull to generate G and h 46 | 47 | ## 🎨 Visualization & Plotting 48 | - [x] GIF generation 49 | - [x] Add subplot 50 | - [x] 3D visualization 51 | - [x] Show the text of the object 52 | - [x] Draw points 53 | - [x] Reformulate plot function - Transform-based plotting architecture 54 | - [x] FOV transform fixes for proper field of view visualization 55 | - [x] Lidar2D plotting with matplotlib transforms 56 | - [x] 3D plotting fixes for patches, lines, and other elements 57 | - [x] Consistent plotting API with state/vertices parameters 58 | - [ ] Draw error band (uncertainty) https://matplotlib.org/stable/gallery/lines_bars_and_markers/curve_error_band.html 59 | - [ ] Academic Color 60 | - [ ] Add some data structure for plot 61 | - [ ] Complete color map 62 | - [ ] Add subwindows for the simulation 63 | 64 | ## 🔧 System & Performance 65 | - [x] Add collision mode 66 | - [x] Add regular event for other obstacles or robots 67 | - [x] Private and public methods and parameters in class 68 | - [x] Add the env logger 69 | - [x] Make the dependency of the package optional 70 | - [ ] Using decorator to update 71 | - [ ] Add synchronization and asynchronization mode 72 | - [ ] Add tf (similar like ROS tf) 73 | - [ ] Rewrite some lib functions by using c++ to improve the efficiency 74 | - [ ] Multiprocess for large scale simulation 75 | - [ ] Make multiple env instances 76 | 77 | ## 🎮 Control & Interaction 78 | - [x] Env res 79 | - [x] Collision check with discrete samples 80 | - [ ] Add more key functions for keyboard control 81 | - [ ] Assign robot goals by mouse click 82 | - [ ] Some judgment functions for control 83 | - [ ] Complete the reactive collision mode 84 | 85 | ## 🧪 Testing & Quality 86 | - [x] Pytest 87 | - [x] Organize the test cases 88 | - [x] Improve coverage of the code over 90% 89 | - [ ] Improve coverage of the code over 99% 90 | - [ ] Test Scenario for the different robot models and planners: pursue and evade, follow, etc. 91 | 92 | ## 📚 Documentation & Examples 93 | - [x] Documentation 94 | - [x] Code annotation for main class 95 | - [x] Add comments for the functions 96 | - [x] Add example yaml files 97 | - [x] Default yaml name (same as python file) 98 | - [x] Reorganize the structure of the readme demonstration 99 | - [x] Argument type hint 100 | - [x] Doc Noise world 101 | - [ ] Doc path manager and change the path 102 | - [ ] Refine documentation and the comment of the code function. 103 | - [ ] Add tutorial with more examples. 104 | 105 | ## 🔗 External Interfaces 106 | - [ ] Add the interface with gym 107 | - [ ] Interface with ROS 108 | - [ ] Interface with Pybullet or Gazebo 109 | - [ ] Add the interface with opendrive 110 | - [ ] Modular yaml import so that ir-sim can read the yaml file separately 111 | 112 | ## 🧠 Advanced Features 113 | - [x] Add ORCA Behavior 114 | - [ ] Add wrapper for ORCA algorithm 115 | - [ ] LLM integration 116 | - [ ] Add various navigation algorithms implemented on the ir-sim 117 | 118 | ## 🚗 Scenarios & Applications 119 | - [ ] Make_scenarios to generate some common scenarios to test, such as car_racing, maze, traffic (Maze generator) 120 | - [ ] Traffic scenarios 121 | 122 | --- 123 | 124 | 125 | --------------------------------------------------------------------------------