├── .gitignore ├── LICENSE ├── MANIFEST.in ├── README.md ├── configs ├── default_bullet.yaml └── default_isaac_gym.yaml ├── pyproject.toml ├── setup.cfg ├── setup.py ├── src └── easysim │ ├── __init__.py │ ├── body.py │ ├── camera.py │ ├── cmd.py │ ├── config.py │ ├── constants.py │ ├── contact.py │ ├── py.typed │ ├── scene.py │ ├── simulator_env.py │ └── simulators │ ├── bullet.py │ ├── isaac_gym.py │ ├── registration.py │ └── simulator.py └── tests ├── __init__.py └── easysim_version_test.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | ####################################################### 6 | # KEEP ITEMS IN ALPHABETICAL ORDER WITHIN THEIR GROUP # 7 | ####################################################### 8 | 9 | # Temporary and binary files 10 | *~ 11 | *.cfg 12 | *.log 13 | *.pot 14 | *.py[cod] 15 | *.orig 16 | *.so 17 | *.swp 18 | */.ipynb_checkpoints/* 19 | .DS_Store 20 | .cache/* 21 | .thumbs/* 22 | __pycache__/* 23 | 24 | # Project files 25 | .ropeproject 26 | .project 27 | .pydevproject 28 | .settings 29 | .idea 30 | .vscode 31 | tags 32 | 33 | # Package files 34 | *.egg 35 | *.eggs/ 36 | .installed.cfg 37 | *.egg-info 38 | 39 | # Unittest and coverage 40 | htmlcov/* 41 | .coverage 42 | .coverage.* 43 | .tox 44 | junit*.xml 45 | coverage.xml 46 | .pytest_cache/ 47 | 48 | # Build and docs folder/files 49 | _build/* 50 | _coverage/* 51 | build/* 52 | dist/* 53 | sdist/* 54 | docs/_api/* 55 | docs/_rst/* 56 | docs/_build/* 57 | cover/* 58 | MANIFEST 59 | 60 | # Per-project virtualenvs 61 | .venv*/ 62 | .conda*/ 63 | venv*/* 64 | 65 | # Jupyter files 66 | *checkpoint.ipynb 67 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 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 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | # This file list the additional files that should be included in the package distribution. 6 | # 7 | # References: 8 | # * https://packaging.python.org/guides/using-manifest-in/ 9 | # * https://setuptools.readthedocs.io/en/latest/userguide/datafiles.html 10 | # * https://stackoverflow.com/questions/6028000/how-to-read-a-static-file-from-inside-a-python-package 11 | 12 | # Marker file for PEP 561 (https://www.python.org/dev/peps/pep-0561/) stating that this package uses inline types. 13 | global-include py.typed 14 | 15 | # graft 16 | prune tests 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EasySim 2 | 3 | ## Prerequisites 4 | 5 | This code is tested with Python 3.8 on Linux. 6 | 7 | ## Installation 8 | 9 | For good practice for Python package management, it is recommended to install the package into a virtual environment (e.g., `virtualenv` or `conda`). 10 | 11 | First clone the repo and install with `pip`'s editable mode. 12 | 13 | ```Shell 14 | git clone git@github.com:NVlabs/easysim.git 15 | cd easysim 16 | pip install -e .[dev] 17 | ``` 18 | -------------------------------------------------------------------------------- /configs/default_bullet.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NVlabs/easysim/19842cc1c03deb03c8ab3be1b19a991594706dec/configs/default_bullet.yaml -------------------------------------------------------------------------------- /configs/default_isaac_gym.yaml: -------------------------------------------------------------------------------- 1 | SIM: 2 | SIMULATOR: isaac_gym 3 | INIT_VIEWER_CAMERA_POSITION: (+3.1375, -2.6327, +2.8679) 4 | INIT_VIEWER_CAMERA_TARGET: (0, 0, 0) 5 | SIM_DEVICE: cuda:0 6 | ISAAC_GYM: 7 | USE_GPU_PIPELINE: True 8 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | [build-system] 6 | requires = ["setuptools>=45", "setuptools_scm>=6.2", "wheel"] 7 | build-backend = "setuptools.build_meta" 8 | 9 | # See the `setuptools_scm` documentation for the description of the schemes used below. 10 | # https://pypi.org/project/setuptools-scm/ 11 | # NOTE: If these values are updated, they need to be also updated in `src/easysim/__init__.py`. 12 | [tool.setuptools_scm] 13 | version_scheme = "no-guess-dev" 14 | local_scheme = "dirty-tag" 15 | 16 | [tool.black] 17 | line-length = 100 18 | experimental-string-processing = true 19 | 20 | [tool.isort] 21 | # Multi line output mode 3 is used to conform with Black. See 22 | # https://github.com/PyCQA/isort#multi-line-output-modes 23 | multi_line_output = 3 24 | profile = "black" 25 | import_heading_stdlib = "Standard Library" 26 | import_heading_thirdparty = "Third Party" 27 | import_heading_firstparty = "SRL" 28 | import_heading_localfolder = "Local Folder" 29 | line_length = 100 30 | 31 | [tool.mypy] 32 | disallow_untyped_defs = true 33 | no_implicit_optional = true 34 | show_error_codes = true 35 | warn_unused_ignores = true 36 | exclude = [ 37 | "venv", 38 | ] 39 | 40 | [[tool.mypy.overrides]] 41 | module = [ 42 | "setuptools", 43 | "setuptools_scm", 44 | ] 45 | ignore_missing_imports = true 46 | 47 | [tool.pytest.ini_options] 48 | python_files = ["*_test.py"] 49 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | # Additional files that need to be included in the package distribution must be 6 | # listed in the MANIFEST.in file. 7 | # 8 | # References: 9 | # * https://newbedev.com/how-include-static-files-to-setuptools-python-package 10 | 11 | [metadata] 12 | 13 | # Configure specific project settings 14 | name = easysim 15 | author = Yu-Wei Chao 16 | description = A library for creating Gym environments with unified API to various physics simulators 17 | url = https://github.com/NVlabs/easysim 18 | license = MIT 19 | 20 | # Configure general project settings 21 | long_description = file: README.md 22 | long_description_content_type = text/markdown 23 | license_files = LICENSE 24 | 25 | # List of classifiers can be found here: 26 | # https://pypi.org/classifiers/ 27 | classifiers = 28 | Intended Audience :: Developers 29 | License :: OSI Approved :: MIT License 30 | Natural Language :: English 31 | Operating System :: OS Independent 32 | Programming Language :: Python :: 3 33 | Topic :: Scientific/Engineering 34 | 35 | # Change if running only on Windows, Mac or Linux (comma-separated) 36 | platforms = any 37 | 38 | [options] 39 | install_requires = 40 | gym 41 | numpy 42 | pybullet 43 | setuptools_scm 44 | torch 45 | yacs 46 | packages = find_namespace: 47 | package_dir = 48 | = src 49 | include_package_data = True 50 | 51 | [options.packages.find] 52 | where = src 53 | 54 | [options.extras_require] 55 | dev = 56 | anybadge==1.8.0 57 | black==21.12b0 58 | build==0.7.0 59 | graphviz==0.19.1 60 | flake8==4.0.1 61 | flake8-copyright==0.2.2 62 | flake8-docstrings==1.6.0 63 | flake8-isort==4.1.1 64 | mypy==0.931 65 | pytest==6.2.5 66 | pytest-cov==3.0.0 67 | sphinx==4.3.0 68 | sphinx_rtd_theme==1.0.0 69 | twine==3.7.1 70 | 71 | [options.entry_points] 72 | # Add here console scripts like: 73 | # console_scripts = 74 | # script_name = package.module:function 75 | 76 | # NOTE (roflaherty): Flake8 doesn't support pyproject.toml configuration yet. 77 | [flake8] 78 | select = E,F,W,C,D,I 79 | copyright-check = True 80 | copyright-regexp = # Copyright \(c\) 20\d{2}(?:-20\d{2])?, NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n#\n# Licensed under the MIT License \[see LICENSE for details\].\n 81 | max-line-length = 100 82 | docstring-convention = google 83 | exclude = build,venv 84 | ignore = 85 | E731 # (ignore recommendation to not use lambdas because there is no other way to write a single line function with out warnings) 86 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | """EasySim package setuptools.""" 6 | 7 | # NOTE (roflaherty): This file is still needed to allow the package to be 8 | # installed in editable mode. 9 | # 10 | # References: 11 | # * https://setuptools.pypa.io/en/latest/setuptools.html#setup-cfg-only-projects 12 | 13 | # Third Party 14 | import setuptools 15 | 16 | setuptools.setup() 17 | -------------------------------------------------------------------------------- /src/easysim/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | """EasySim package.""" 6 | 7 | 8 | # NOTE (roflaherty): This is inspired by how matplotlib does creates its version value. 9 | # https://github.com/matplotlib/matplotlib/blob/master/lib/matplotlib/__init__.py#L161 10 | def _get_version() -> str: 11 | """Return the version string used for __version__.""" 12 | # Standard Library 13 | import pathlib 14 | 15 | root = pathlib.Path(__file__).resolve().parent.parent.parent 16 | if (root / ".git").exists() and not (root / ".git/shallow").exists(): 17 | # Third Party 18 | import setuptools_scm 19 | 20 | this_version: str 21 | # See the `setuptools_scm` documentation for the description of the schemes used below. 22 | # https://pypi.org/project/setuptools-scm/ 23 | # NOTE: If these values are updated, they need to be also updated in `pyproject.toml`. 24 | this_version = setuptools_scm.get_version( 25 | root=root, 26 | version_scheme="no-guess-dev", 27 | local_scheme="dirty-tag", 28 | ) 29 | else: # Get the version from the _version.py setuptools_scm file. 30 | try: 31 | # Standard Library 32 | from importlib.metadata import version 33 | except ModuleNotFoundError: 34 | # NOTE: `importlib.resources` is part of the standard library in Python 3.9. 35 | # `importlib_metadata` is the back ported library for older versions of python. 36 | # Third Party 37 | from importlib_metadata import version # type: ignore[no-redef] 38 | 39 | this_version = version("easysim") 40 | 41 | return this_version 42 | 43 | 44 | # Set `__version__` attribute 45 | __version__ = _get_version() 46 | 47 | # Remove `_get_version` so it is not added as an attribute 48 | del _get_version 49 | 50 | from easysim.simulator_env import SimulatorEnv, SimulatorWrapper 51 | from easysim.body import Body 52 | from easysim.camera import Camera 53 | from easysim.constants import GeometryType, DoFControlMode, MeshNormalMode 54 | from easysim.config import cfg, get_cfg 55 | from easysim.cmd import get_config_from_args 56 | 57 | __all__ = [ 58 | "SimulatorEnv", 59 | "SimulatorWrapper", 60 | "Body", 61 | "Camera", 62 | "GeometryType", 63 | "DoFControlMode", 64 | "MeshNormalMode", 65 | "cfg", 66 | "get_cfg", 67 | "get_config_from_args", 68 | ] 69 | -------------------------------------------------------------------------------- /src/easysim/body.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import torch 6 | import numpy as np 7 | 8 | from contextlib import contextmanager 9 | 10 | 11 | class Body: 12 | """ """ 13 | 14 | _ATTR_TENSOR_NDIM = { 15 | "initial_base_position": 1, 16 | "initial_base_velocity": 1, 17 | "initial_dof_position": 1, 18 | "initial_dof_velocity": 1, 19 | "dof_target_position": 1, 20 | "dof_target_velocity": 1, 21 | "dof_force": 1, 22 | } 23 | _ATTR_ARRAY_NDIM = { 24 | "scale": 0, 25 | "link_collision_filter": 1, 26 | "link_lateral_friction": 1, 27 | "link_spinning_friction": 1, 28 | "link_rolling_friction": 1, 29 | "link_restitution": 1, 30 | "link_linear_damping": 0, 31 | "link_angular_damping": 0, 32 | "link_color": 2, 33 | "link_segmentation_id": 1, 34 | "dof_has_limits": 1, 35 | "dof_lower_limit": 1, 36 | "dof_upper_limit": 1, 37 | "dof_control_mode": 0, 38 | "dof_max_force": 1, 39 | "dof_max_velocity": 1, 40 | "dof_position_gain": 1, 41 | "dof_velocity_gain": 1, 42 | "dof_armature": 1, 43 | } 44 | 45 | _created = False 46 | 47 | def __init__( 48 | self, 49 | name=None, 50 | geometry_type=None, 51 | urdf_file=None, 52 | sphere_radius=None, 53 | box_half_extent=None, 54 | device=None, 55 | use_fixed_base=None, 56 | use_self_collision=None, 57 | vhacd_enabled=None, 58 | vhacd_params=None, 59 | mesh_normal_mode=None, 60 | env_ids_load=None, 61 | initial_base_position=None, 62 | initial_base_velocity=None, 63 | initial_dof_position=None, 64 | initial_dof_velocity=None, 65 | scale=None, 66 | link_collision_filter=None, 67 | link_lateral_friction=None, 68 | link_spinning_friction=None, 69 | link_rolling_friction=None, 70 | link_restitution=None, 71 | link_linear_damping=None, 72 | link_angular_damping=None, 73 | link_color=None, 74 | link_segmentation_id=None, 75 | dof_has_limits=None, 76 | dof_lower_limit=None, 77 | dof_upper_limit=None, 78 | dof_control_mode=None, 79 | dof_max_force=None, 80 | dof_max_velocity=None, 81 | dof_position_gain=None, 82 | dof_velocity_gain=None, 83 | dof_armature=None, 84 | dof_target_position=None, 85 | dof_target_velocity=None, 86 | dof_force=None, 87 | ): 88 | """ """ 89 | self._init_attr_array_pipeline() 90 | self._init_callback() 91 | 92 | self.name = name 93 | self.geometry_type = geometry_type 94 | self.urdf_file = urdf_file 95 | self.sphere_radius = sphere_radius 96 | self.box_half_extent = box_half_extent 97 | self.device = device 98 | self.use_fixed_base = use_fixed_base 99 | self.use_self_collision = use_self_collision 100 | self.vhacd_enabled = vhacd_enabled 101 | self.vhacd_params = vhacd_params 102 | self.mesh_normal_mode = mesh_normal_mode 103 | self.env_ids_load = env_ids_load 104 | self.initial_base_position = initial_base_position 105 | self.initial_base_velocity = initial_base_velocity 106 | self.initial_dof_position = initial_dof_position 107 | self.initial_dof_velocity = initial_dof_velocity 108 | 109 | self.scale = scale 110 | 111 | self.link_collision_filter = link_collision_filter 112 | self.link_lateral_friction = link_lateral_friction 113 | self.link_spinning_friction = link_spinning_friction 114 | self.link_rolling_friction = link_rolling_friction 115 | self.link_restitution = link_restitution 116 | self.link_linear_damping = link_linear_damping 117 | self.link_angular_damping = link_angular_damping 118 | 119 | self.link_color = link_color 120 | self.link_segmentation_id = link_segmentation_id 121 | 122 | self.dof_has_limits = dof_has_limits 123 | self.dof_lower_limit = dof_lower_limit 124 | self.dof_upper_limit = dof_upper_limit 125 | self.dof_control_mode = dof_control_mode 126 | self.dof_max_force = dof_max_force 127 | self.dof_max_velocity = dof_max_velocity 128 | self.dof_position_gain = dof_position_gain 129 | self.dof_velocity_gain = dof_velocity_gain 130 | self.dof_armature = dof_armature 131 | 132 | self.dof_target_position = dof_target_position 133 | self.dof_target_velocity = dof_target_velocity 134 | self.dof_force = dof_force 135 | 136 | self.env_ids_reset_base_state = None 137 | self.env_ids_reset_dof_state = None 138 | 139 | self.dof_state = None 140 | self.link_state = None 141 | self.contact_id = None 142 | 143 | self._created = True 144 | 145 | def __setattr__(self, key, value): 146 | """ """ 147 | # Exclude `dof_state` and `link_state` to prevent infinite recursion in property calls. 148 | if self._created and key not in ("dof_state", "link_state") and not hasattr(self, key): 149 | raise TypeError(f"Unrecognized Body attribute '{key}': {self.name}") 150 | object.__setattr__(self, key, value) 151 | 152 | def _init_attr_array_pipeline(self): 153 | """ """ 154 | self._attr_array_locked = {} 155 | self._attr_array_dirty_flag = {} 156 | self._attr_array_dirty_mask = {} 157 | self._attr_array_default_flag = {} 158 | for attr in self._ATTR_ARRAY_NDIM: 159 | self._attr_array_locked[attr] = False 160 | self._attr_array_dirty_flag[attr] = False 161 | self._attr_array_default_flag[attr] = False 162 | 163 | @property 164 | def attr_array_locked(self): 165 | """ """ 166 | return self._attr_array_locked 167 | 168 | @property 169 | def attr_array_dirty_flag(self): 170 | """ """ 171 | return self._attr_array_dirty_flag 172 | 173 | @property 174 | def attr_array_dirty_mask(self): 175 | """ """ 176 | return self._attr_array_dirty_mask 177 | 178 | @property 179 | def attr_array_default_flag(self): 180 | """ """ 181 | return self._attr_array_default_flag 182 | 183 | def _init_callback(self): 184 | """ """ 185 | self._callback_collect_dof_state = None 186 | self._callback_collect_link_state = None 187 | 188 | @property 189 | def name(self): 190 | """ """ 191 | return self._name 192 | 193 | @name.setter 194 | def name(self, value): 195 | """ """ 196 | self._name = value 197 | 198 | @property 199 | def geometry_type(self): 200 | """ """ 201 | return self._geometry_type 202 | 203 | @geometry_type.setter 204 | def geometry_type(self, value): 205 | """ """ 206 | self._geometry_type = value 207 | 208 | @property 209 | def urdf_file(self): 210 | """ """ 211 | return self._urdf_file 212 | 213 | @urdf_file.setter 214 | def urdf_file(self, value): 215 | """ """ 216 | self._urdf_file = value 217 | 218 | @property 219 | def sphere_radius(self): 220 | """ """ 221 | return self._sphere_radius 222 | 223 | @sphere_radius.setter 224 | def sphere_radius(self, value): 225 | """ """ 226 | self._sphere_radius = value 227 | 228 | @property 229 | def box_half_extent(self): 230 | """ """ 231 | return self._box_half_extent 232 | 233 | @box_half_extent.setter 234 | def box_half_extent(self, value): 235 | """ """ 236 | self._box_half_extent = value 237 | 238 | @property 239 | def device(self): 240 | """ """ 241 | return self._device 242 | 243 | @device.setter 244 | def device(self, value): 245 | """ """ 246 | self._device = value 247 | 248 | if hasattr(self, "_env_ids_load") and self.env_ids_load is not None: 249 | self.env_ids_load = self.env_ids_load.to(value) 250 | 251 | if hasattr(self, "_initial_base_position") and self.initial_base_position is not None: 252 | self.initial_base_position = self.initial_base_position.to(value) 253 | if hasattr(self, "_initial_base_velocity") and self.initial_base_velocity is not None: 254 | self.initial_base_velocity = self.initial_base_velocity.to(value) 255 | if hasattr(self, "_initial_dof_position") and self.initial_dof_position is not None: 256 | self.initial_dof_position = self.initial_dof_position.to(value) 257 | if hasattr(self, "_initial_dof_velocity") and self.initial_dof_velocity is not None: 258 | self.initial_dof_velocity = self.initial_dof_velocity.to(value) 259 | 260 | if hasattr(self, "_dof_target_position") and self.dof_target_position is not None: 261 | self.dof_target_position = self.dof_target_position.to(value) 262 | if hasattr(self, "_dof_target_velocity") and self.dof_target_velocity is not None: 263 | self.dof_target_velocity = self.dof_target_velocity.to(value) 264 | if hasattr(self, "_dof_force") and self.dof_force is not None: 265 | self.dof_force = self.dof_force.to(value) 266 | 267 | if hasattr(self, "_env_ids_reset_dof_state") and self.env_ids_reset_dof_state is not None: 268 | self.env_ids_reset_dof_state = self.env_ids_reset_dof_state.to(value) 269 | 270 | if hasattr(self, "_dof_state") and self.dof_state is not None: 271 | self.dof_state = self.dof_state.to(value) 272 | if hasattr(self, "_link_state") and self.link_state is not None: 273 | self.link_state = self.link_state.to(value) 274 | 275 | @property 276 | def use_fixed_base(self): 277 | """ """ 278 | return self._use_fixed_base 279 | 280 | @use_fixed_base.setter 281 | def use_fixed_base(self, value): 282 | """ """ 283 | self._use_fixed_base = value 284 | 285 | @property 286 | def use_self_collision(self): 287 | """ """ 288 | return self._use_self_collision 289 | 290 | @use_self_collision.setter 291 | def use_self_collision(self, value): 292 | """ """ 293 | self._use_self_collision = value 294 | 295 | @property 296 | def vhacd_enabled(self): 297 | """ """ 298 | return self._vhacd_enabled 299 | 300 | @vhacd_enabled.setter 301 | def vhacd_enabled(self, value): 302 | """ """ 303 | self._vhacd_enabled = value 304 | 305 | @property 306 | def vhacd_params(self): 307 | """ """ 308 | return self._vhacd_params 309 | 310 | @vhacd_params.setter 311 | def vhacd_params(self, value): 312 | """ """ 313 | self._vhacd_params = value 314 | 315 | @property 316 | def mesh_normal_mode(self): 317 | """ """ 318 | return self._mesh_normal_mode 319 | 320 | @mesh_normal_mode.setter 321 | def mesh_normal_mode(self, value): 322 | """ """ 323 | self._mesh_normal_mode = value 324 | 325 | @property 326 | def env_ids_load(self): 327 | """ """ 328 | return self._env_ids_load 329 | 330 | @env_ids_load.setter 331 | def env_ids_load(self, value): 332 | """ """ 333 | if value is not None: 334 | value = torch.as_tensor(value, dtype=torch.int64, device=self.device) 335 | if value.ndim != 1: 336 | raise ValueError("'env_ids_load' must have a number of dimensions of 1") 337 | self._env_ids_load = value 338 | 339 | @property 340 | def initial_base_position(self): 341 | """ """ 342 | return self._initial_base_position 343 | 344 | @initial_base_position.setter 345 | def initial_base_position(self, value): 346 | """ """ 347 | if value is not None: 348 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 349 | if value.ndim not in ( 350 | self._ATTR_TENSOR_NDIM["initial_base_position"], 351 | self._ATTR_TENSOR_NDIM["initial_base_position"] + 1, 352 | ): 353 | raise ValueError( 354 | "'initial_base_position' must have a number of dimensions of " 355 | f"{self._ATTR_TENSOR_NDIM['initial_base_position']} or " 356 | f"{self._ATTR_TENSOR_NDIM['initial_base_position'] + 1}" 357 | ) 358 | if value.shape[-1] != 7: 359 | raise ValueError("'initial_base_position' must have the last dimension of size 7") 360 | self._initial_base_position = value 361 | 362 | @property 363 | def initial_base_velocity(self): 364 | """ """ 365 | return self._initial_base_velocity 366 | 367 | @initial_base_velocity.setter 368 | def initial_base_velocity(self, value): 369 | """ """ 370 | if value is not None: 371 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 372 | if value.ndim not in ( 373 | self._ATTR_TENSOR_NDIM["initial_base_velocity"], 374 | self._ATTR_TENSOR_NDIM["initial_base_velocity"] + 1, 375 | ): 376 | raise ValueError( 377 | "'initial_base_velocity' must have a number of dimensions of " 378 | f"{self._ATTR_TENSOR_NDIM['initial_base_velocity']} or " 379 | f"{self._ATTR_TENSOR_NDIM['initial_base_velocity'] + 1}" 380 | ) 381 | if value.shape[-1] != 6: 382 | raise ValueError("'initial_base_velocity' must have the last dimension of size 6") 383 | self._initial_base_velocity = value 384 | 385 | @property 386 | def initial_dof_position(self): 387 | """ """ 388 | return self._initial_dof_position 389 | 390 | @initial_dof_position.setter 391 | def initial_dof_position(self, value): 392 | """ """ 393 | if value is not None: 394 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 395 | if value.ndim not in ( 396 | self._ATTR_TENSOR_NDIM["initial_dof_position"], 397 | self._ATTR_TENSOR_NDIM["initial_dof_position"] + 1, 398 | ): 399 | raise ValueError( 400 | "'initial_dof_position' must have a number of dimensions of " 401 | f"{self._ATTR_TENSOR_NDIM['initial_dof_position']} or " 402 | f"{self._ATTR_TENSOR_NDIM['initial_dof_position'] + 1}" 403 | ) 404 | self._initial_dof_position = value 405 | 406 | @property 407 | def initial_dof_velocity(self): 408 | """ """ 409 | return self._initial_dof_velocity 410 | 411 | @initial_dof_velocity.setter 412 | def initial_dof_velocity(self, value): 413 | """ """ 414 | if value is not None: 415 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 416 | if value.ndim not in ( 417 | self._ATTR_TENSOR_NDIM["initial_dof_velocity"], 418 | self._ATTR_TENSOR_NDIM["initial_dof_velocity"] + 1, 419 | ): 420 | raise ValueError( 421 | "'initial_dof_velocity' must have a number of dimensions of " 422 | f"{self._ATTR_TENSOR_NDIM['initial_dof_velocity']} or " 423 | f"{self._ATTR_TENSOR_NDIM['initial_dof_velocity'] + 1}" 424 | ) 425 | self._initial_dof_velocity = value 426 | 427 | @property 428 | def scale(self): 429 | """ """ 430 | return self._scale 431 | 432 | @scale.setter 433 | def scale(self, value): 434 | """ """ 435 | assert not self._attr_array_locked[ 436 | "scale" 437 | ], "'scale' cannot be directly changed after simulation starts. Use 'update_attr_array()'." 438 | if value is not None: 439 | value = np.asanyarray(value, dtype=np.float32) 440 | if value.ndim not in ( 441 | self._ATTR_ARRAY_NDIM["scale"], 442 | self._ATTR_ARRAY_NDIM["scale"] + 1, 443 | ): 444 | raise ValueError( 445 | f"'scale' must have a number of dimensions of {self._ATTR_ARRAY_NDIM['scale']} " 446 | f"or {self._ATTR_ARRAY_NDIM['scale'] + 1}" 447 | ) 448 | self._scale = value 449 | 450 | @property 451 | def link_collision_filter(self): 452 | """ """ 453 | return self._link_collision_filter 454 | 455 | @link_collision_filter.setter 456 | def link_collision_filter(self, value): 457 | """ """ 458 | assert not self._attr_array_locked["link_collision_filter"], ( 459 | "'link_collision_filter' cannot be directly changed after simulation starts. Use " 460 | "'update_attr_array()'." 461 | ) 462 | if value is not None: 463 | value = np.asanyarray(value, dtype=np.int64) 464 | if value.ndim not in ( 465 | self._ATTR_ARRAY_NDIM["link_collision_filter"], 466 | self._ATTR_ARRAY_NDIM["link_collision_filter"] + 1, 467 | ): 468 | raise ValueError( 469 | "'link_collision_filter' must have a number of dimensions of " 470 | f"{self._ATTR_ARRAY_NDIM['link_collision_filter']} or " 471 | f"{self._ATTR_ARRAY_NDIM['link_collision_filter'] + 1}" 472 | ) 473 | self._link_collision_filter = value 474 | 475 | @property 476 | def link_lateral_friction(self): 477 | """ """ 478 | return self._link_lateral_friction 479 | 480 | @link_lateral_friction.setter 481 | def link_lateral_friction(self, value): 482 | """ """ 483 | assert not self._attr_array_locked["link_lateral_friction"], ( 484 | "'link_lateral_friction' cannot be directly changed after simulation starts. Use " 485 | "'update_attr_array()'." 486 | ) 487 | if value is not None: 488 | value = np.asanyarray(value, dtype=np.float32) 489 | if value.ndim not in ( 490 | self._ATTR_ARRAY_NDIM["link_lateral_friction"], 491 | self._ATTR_ARRAY_NDIM["link_lateral_friction"] + 1, 492 | ): 493 | raise ValueError( 494 | "'link_lateral_friction' must have a number of dimensions of " 495 | f"{self._ATTR_ARRAY_NDIM['link_lateral_friction']} or " 496 | f"{self._ATTR_ARRAY_NDIM['link_lateral_friction'] + 1}" 497 | ) 498 | self._link_lateral_friction = value 499 | 500 | @property 501 | def link_spinning_friction(self): 502 | """ """ 503 | return self._link_spinning_friction 504 | 505 | @link_spinning_friction.setter 506 | def link_spinning_friction(self, value): 507 | """ """ 508 | assert not self._attr_array_locked["link_spinning_friction"], ( 509 | "'link_spinning_friction' cannot be directly changed after simulation starts. Use " 510 | "'update_attr_array()'." 511 | ) 512 | if value is not None: 513 | value = np.asanyarray(value, dtype=np.float32) 514 | if value.ndim not in ( 515 | self._ATTR_ARRAY_NDIM["link_spinning_friction"], 516 | self._ATTR_ARRAY_NDIM["link_spinning_friction"] + 1, 517 | ): 518 | raise ValueError( 519 | "'link_spinning_friction' must have a number of dimensions of " 520 | f"{self._ATTR_ARRAY_NDIM['link_spinning_friction']} or " 521 | f"{self._ATTR_ARRAY_NDIM['link_spinning_friction'] + 1}" 522 | ) 523 | self._link_spinning_friction = value 524 | 525 | @property 526 | def link_rolling_friction(self): 527 | """ """ 528 | return self._link_rolling_friction 529 | 530 | @link_rolling_friction.setter 531 | def link_rolling_friction(self, value): 532 | """ """ 533 | assert not self._attr_array_locked["link_rolling_friction"], ( 534 | "'link_rolling_friction' cannot be directly changed after simulation starts. Use " 535 | "'update_attr_array()'." 536 | ) 537 | if value is not None: 538 | value = np.asanyarray(value, dtype=np.float32) 539 | if value.ndim not in ( 540 | self._ATTR_ARRAY_NDIM["link_rolling_friction"], 541 | self._ATTR_ARRAY_NDIM["link_rolling_friction"] + 1, 542 | ): 543 | raise ValueError( 544 | "'link_rolling_friction' must have a number of dimensions of " 545 | f"{self._ATTR_ARRAY_NDIM['link_rolling_friction']} or " 546 | f"{self._ATTR_ARRAY_NDIM['link_rolling_friction'] + 1}" 547 | ) 548 | self._link_rolling_friction = value 549 | 550 | @property 551 | def link_restitution(self): 552 | """ """ 553 | return self._link_restitution 554 | 555 | @link_restitution.setter 556 | def link_restitution(self, value): 557 | """ """ 558 | assert not self._attr_array_locked["link_restitution"], ( 559 | "'link_restitution' cannot be directly changed after simulation starts. Use " 560 | "'update_attr_array()'." 561 | ) 562 | if value is not None: 563 | value = np.asanyarray(value, dtype=np.float32) 564 | if value.ndim not in ( 565 | self._ATTR_ARRAY_NDIM["link_restitution"], 566 | self._ATTR_ARRAY_NDIM["link_restitution"] + 1, 567 | ): 568 | raise ValueError( 569 | "'link_restitution' must have a number of dimensions of " 570 | f"{self._ATTR_ARRAY_NDIM['link_restitution']} or " 571 | f"{self._ATTR_ARRAY_NDIM['link_restitution'] + 1}" 572 | ) 573 | self._link_restitution = value 574 | 575 | @property 576 | def link_linear_damping(self): 577 | """ """ 578 | return self._link_linear_damping 579 | 580 | @link_linear_damping.setter 581 | def link_linear_damping(self, value): 582 | """ """ 583 | assert not self._attr_array_locked["link_linear_damping"], ( 584 | "'link_linear_damping' cannot be directly changed after simulation starts. Use " 585 | "'update_attr_array()'." 586 | ) 587 | if value is not None: 588 | value = np.asanyarray(value, dtype=np.float32) 589 | if value.ndim not in ( 590 | self._ATTR_ARRAY_NDIM["link_linear_damping"], 591 | self._ATTR_ARRAY_NDIM["link_linear_damping"] + 1, 592 | ): 593 | raise ValueError( 594 | "'link_linear_damping' must have a number of dimensions of " 595 | f"{self._ATTR_ARRAY_NDIM['link_linear_damping']} or " 596 | f"{self._ATTR_ARRAY_NDIM['link_linear_damping'] + 1}" 597 | ) 598 | self._link_linear_damping = value 599 | 600 | @property 601 | def link_angular_damping(self): 602 | """ """ 603 | return self._link_angular_damping 604 | 605 | @link_angular_damping.setter 606 | def link_angular_damping(self, value): 607 | """ """ 608 | assert not self._attr_array_locked["link_angular_damping"], ( 609 | "'link_angular_damping' cannot be directly changed after simulation starts. Use " 610 | "'update_attr_array()'." 611 | ) 612 | if value is not None: 613 | value = np.asanyarray(value, dtype=np.float32) 614 | if value.ndim not in ( 615 | self._ATTR_ARRAY_NDIM["link_angular_damping"], 616 | self._ATTR_ARRAY_NDIM["link_angular_damping"] + 1, 617 | ): 618 | raise ValueError( 619 | "'link_angular_damping' must have a number of dimensions of " 620 | f"{self._ATTR_ARRAY_NDIM['link_angular_damping']} or " 621 | f"{self._ATTR_ARRAY_NDIM['link_angular_damping'] + 1}" 622 | ) 623 | self._link_angular_damping = value 624 | 625 | @property 626 | def link_color(self): 627 | """ """ 628 | return self._link_color 629 | 630 | @link_color.setter 631 | def link_color(self, value): 632 | """ """ 633 | assert not self._attr_array_locked["link_color"], ( 634 | "'link_color' cannot be directly changed after simulation starts. Use " 635 | "'update_attr_array()'." 636 | ) 637 | if value is not None: 638 | value = np.asanyarray(value, dtype=np.float32) 639 | if value.ndim not in ( 640 | self._ATTR_ARRAY_NDIM["link_color"], 641 | self._ATTR_ARRAY_NDIM["link_color"] + 1, 642 | ): 643 | raise ValueError( 644 | "'link_color' must have a number of dimensions of " 645 | f"{self._ATTR_ARRAY_NDIM['link_color']} or " 646 | f"{self._ATTR_ARRAY_NDIM['link_color'] + 1}" 647 | ) 648 | self._link_color = value 649 | 650 | @property 651 | def link_segmentation_id(self): 652 | """ """ 653 | return self._link_segmentation_id 654 | 655 | @link_segmentation_id.setter 656 | def link_segmentation_id(self, value): 657 | """ """ 658 | assert not self._attr_array_locked["link_segmentation_id"], ( 659 | "'link_segmentation_id' cannot be directly changed after simulation starts. Use " 660 | "'update_attr_array()'." 661 | ) 662 | if value is not None: 663 | value = np.asanyarray(value, dtype=np.int64) 664 | if value.ndim not in ( 665 | self._ATTR_ARRAY_NDIM["link_segmentation_id"], 666 | self._ATTR_ARRAY_NDIM["link_segmentation_id"] + 1, 667 | ): 668 | raise ValueError( 669 | "'link_segmentation_id' must have a number of dimensions of " 670 | f"{self._ATTR_ARRAY_NDIM['link_segmentation_id']} or " 671 | f"{self._ATTR_ARRAY_NDIM['link_segmentation_id'] + 1}" 672 | ) 673 | self._link_segmentation_id = value 674 | 675 | @property 676 | def dof_has_limits(self): 677 | """ """ 678 | return self._dof_has_limits 679 | 680 | @dof_has_limits.setter 681 | def dof_has_limits(self, value): 682 | """ """ 683 | assert not self._attr_array_locked["dof_has_limits"], ( 684 | "'dof_has_limits' cannot be directly changed after simulation starts. Use " 685 | "'update_attr_array()'." 686 | ) 687 | if value is not None: 688 | value = np.asanyarray(value, dtype=np.bool_) 689 | if value.ndim not in ( 690 | self._ATTR_ARRAY_NDIM["dof_has_limits"], 691 | self._ATTR_ARRAY_NDIM["dof_has_limits"] + 1, 692 | ): 693 | raise ValueError( 694 | "'dof_has_limits' must have a number of dimensions of " 695 | f"{self._ATTR_ARRAY_NDIM['dof_has_limits']} or " 696 | f"{self._ATTR_ARRAY_NDIM['dof_has_limits'] + 1}" 697 | ) 698 | self._dof_has_limits = value 699 | 700 | @property 701 | def dof_lower_limit(self): 702 | """ """ 703 | return self._dof_lower_limit 704 | 705 | @dof_lower_limit.setter 706 | def dof_lower_limit(self, value): 707 | """ """ 708 | assert not self._attr_array_locked["dof_lower_limit"], ( 709 | "'dof_lower_limit' cannot be directly changed after simulation starts. Use " 710 | "'update_attr_array()'." 711 | ) 712 | if value is not None: 713 | value = np.asanyarray(value, dtype=np.float32) 714 | if value.ndim not in ( 715 | self._ATTR_ARRAY_NDIM["dof_lower_limit"], 716 | self._ATTR_ARRAY_NDIM["dof_lower_limit"] + 1, 717 | ): 718 | raise ValueError( 719 | "'dof_lower_limit' must have a number of dimensions of " 720 | f"{self._ATTR_ARRAY_NDIM['dof_lower_limit']} or " 721 | f"{self._ATTR_ARRAY_NDIM['dof_lower_limit'] + 1}" 722 | ) 723 | self._dof_lower_limit = value 724 | 725 | @property 726 | def dof_upper_limit(self): 727 | """ """ 728 | return self._dof_upper_limit 729 | 730 | @dof_upper_limit.setter 731 | def dof_upper_limit(self, value): 732 | """ """ 733 | assert not self._attr_array_locked["dof_upper_limit"], ( 734 | "'dof_upper_limit' cannot be directly changed after simulation starts. Use " 735 | "'update_attr_array()'." 736 | ) 737 | if value is not None: 738 | value = np.asanyarray(value, dtype=np.float32) 739 | if value.ndim not in ( 740 | self._ATTR_ARRAY_NDIM["dof_upper_limit"], 741 | self._ATTR_ARRAY_NDIM["dof_upper_limit"] + 1, 742 | ): 743 | raise ValueError( 744 | "'dof_upper_limit' must have a number of dimensions of " 745 | f"{self._ATTR_ARRAY_NDIM['dof_upper_limit']} or " 746 | f"{self._ATTR_ARRAY_NDIM['dof_upper_limit'] + 1}" 747 | ) 748 | self._dof_upper_limit = value 749 | 750 | @property 751 | def dof_control_mode(self): 752 | """ """ 753 | return self._dof_control_mode 754 | 755 | @dof_control_mode.setter 756 | def dof_control_mode(self, value): 757 | """ """ 758 | assert not self._attr_array_locked["dof_control_mode"], ( 759 | "'dof_control_mode' cannot be directly changed after simulation starts. Use " 760 | "'update_attr_array()'." 761 | ) 762 | if value is not None: 763 | value = np.asanyarray(value, dtype=np.int64) 764 | if value.ndim not in ( 765 | self._ATTR_ARRAY_NDIM["dof_control_mode"], 766 | self._ATTR_ARRAY_NDIM["dof_control_mode"] + 1, 767 | ): 768 | raise ValueError( 769 | "'dof_control_mode' must have a number of dimensions of " 770 | f"{self._ATTR_ARRAY_NDIM['dof_control_mode']} or " 771 | f"{self._ATTR_ARRAY_NDIM['dof_control_mode'] + 1}" 772 | ) 773 | self._dof_control_mode = value 774 | 775 | @property 776 | def dof_max_force(self): 777 | """ """ 778 | return self._dof_max_force 779 | 780 | @dof_max_force.setter 781 | def dof_max_force(self, value): 782 | """ """ 783 | assert not self._attr_array_locked["dof_max_force"], ( 784 | "'dof_max_force' cannot be directly changed after simulation starts. Use " 785 | "'update_attr_array()'." 786 | ) 787 | if value is not None: 788 | value = np.asanyarray(value, dtype=np.float32) 789 | if value.ndim not in ( 790 | self._ATTR_ARRAY_NDIM["dof_max_force"], 791 | self._ATTR_ARRAY_NDIM["dof_max_force"] + 1, 792 | ): 793 | raise ValueError( 794 | "'dof_max_force' must have a number of dimensions of " 795 | f"{self._ATTR_ARRAY_NDIM['dof_max_force']} or " 796 | f"{self._ATTR_ARRAY_NDIM['dof_max_force'] + 1}" 797 | ) 798 | self._dof_max_force = value 799 | 800 | @property 801 | def dof_max_velocity(self): 802 | """ """ 803 | return self._dof_max_velocity 804 | 805 | @dof_max_velocity.setter 806 | def dof_max_velocity(self, value): 807 | """ """ 808 | assert not self._attr_array_locked["dof_max_velocity"], ( 809 | "'dof_max_velocity' cannot be directly changed after simulation starts. Use " 810 | "'update_attr_array()'." 811 | ) 812 | if value is not None: 813 | value = np.asanyarray(value, dtype=np.float32) 814 | if value.ndim not in ( 815 | self._ATTR_ARRAY_NDIM["dof_max_velocity"], 816 | self._ATTR_ARRAY_NDIM["dof_max_velocity"] + 1, 817 | ): 818 | raise ValueError( 819 | "'dof_max_velocity' must have a number of dimensions of " 820 | f"{self._ATTR_ARRAY_NDIM['dof_max_velocity']} or " 821 | f"{self._ATTR_ARRAY_NDIM['dof_max_velocity'] + 1}" 822 | ) 823 | self._dof_max_velocity = value 824 | 825 | @property 826 | def dof_position_gain(self): 827 | """ """ 828 | return self._dof_position_gain 829 | 830 | @dof_position_gain.setter 831 | def dof_position_gain(self, value): 832 | """ """ 833 | assert not self._attr_array_locked["dof_position_gain"], ( 834 | "'dof_position_gain' cannot be directly changed after simulation starts. Use " 835 | "'update_attr_array()'." 836 | ) 837 | if value is not None: 838 | value = np.asanyarray(value, dtype=np.float32) 839 | if value.ndim not in ( 840 | self._ATTR_ARRAY_NDIM["dof_position_gain"], 841 | self._ATTR_ARRAY_NDIM["dof_position_gain"] + 1, 842 | ): 843 | raise ValueError( 844 | "'dof_position_gain' must have a number of dimensions of " 845 | f"{self._ATTR_ARRAY_NDIM['dof_position_gain']} or " 846 | f"{self._ATTR_ARRAY_NDIM['dof_position_gain'] + 1}" 847 | ) 848 | self._dof_position_gain = value 849 | 850 | @property 851 | def dof_velocity_gain(self): 852 | """ """ 853 | return self._dof_velocity_gain 854 | 855 | @dof_velocity_gain.setter 856 | def dof_velocity_gain(self, value): 857 | """ """ 858 | assert not self._attr_array_locked["dof_velocity_gain"], ( 859 | "'dof_velocity_gain' cannot be directly changed after simulation starts. Use " 860 | "'update_attr_array()'." 861 | ) 862 | if value is not None: 863 | value = np.asanyarray(value, dtype=np.float32) 864 | if value.ndim not in ( 865 | self._ATTR_ARRAY_NDIM["dof_velocity_gain"], 866 | self._ATTR_ARRAY_NDIM["dof_velocity_gain"] + 1, 867 | ): 868 | raise ValueError( 869 | "'dof_velocity_gain' must have a number of dimensions of " 870 | f"{self._ATTR_ARRAY_NDIM['dof_velocity_gain']} or " 871 | f"{self._ATTR_ARRAY_NDIM['dof_velocity_gain'] + 1}" 872 | ) 873 | self._dof_velocity_gain = value 874 | 875 | @property 876 | def dof_armature(self): 877 | """ """ 878 | return self._dof_armature 879 | 880 | @dof_armature.setter 881 | def dof_armature(self, value): 882 | """ """ 883 | assert not self._attr_array_locked["dof_armature"], ( 884 | "'dof_armature' cannot be directly changed after simulation starts. Use " 885 | "'update_attr_array()'." 886 | ) 887 | if value is not None: 888 | value = np.asanyarray(value, dtype=np.float32) 889 | if value.ndim not in ( 890 | self._ATTR_ARRAY_NDIM["dof_armature"], 891 | self._ATTR_ARRAY_NDIM["dof_armature"] + 1, 892 | ): 893 | raise ValueError( 894 | "'dof_armature' must have a number of dimensions of " 895 | f"{self._ATTR_ARRAY_NDIM['dof_armature']} or " 896 | f"{self._ATTR_ARRAY_NDIM['dof_armature'] + 1}" 897 | ) 898 | self._dof_armature = value 899 | 900 | @property 901 | def dof_target_position(self): 902 | """ """ 903 | return self._dof_target_position 904 | 905 | @dof_target_position.setter 906 | def dof_target_position(self, value): 907 | """ """ 908 | if value is not None: 909 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 910 | if value.ndim not in ( 911 | self._ATTR_TENSOR_NDIM["dof_target_position"], 912 | self._ATTR_TENSOR_NDIM["dof_target_position"] + 1, 913 | ): 914 | raise ValueError( 915 | "'dof_target_position' must have a number of dimensions of " 916 | f"{self._ATTR_TENSOR_NDIM['dof_target_position']} or " 917 | f"{self._ATTR_TENSOR_NDIM['dof_target_position'] + 1}" 918 | ) 919 | self._dof_target_position = value 920 | 921 | @property 922 | def dof_target_velocity(self): 923 | """ """ 924 | return self._dof_target_velocity 925 | 926 | @dof_target_velocity.setter 927 | def dof_target_velocity(self, value): 928 | """ """ 929 | if value is not None: 930 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 931 | if value.ndim not in ( 932 | self._ATTR_TENSOR_NDIM["dof_target_velocity"], 933 | self._ATTR_TENSOR_NDIM["dof_target_velocity"] + 1, 934 | ): 935 | raise ValueError( 936 | "'dof_target_velocity' must have a number of dimensions of " 937 | f"{self._ATTR_TENSOR_NDIM['dof_target_velocity']} or " 938 | f"{self._ATTR_TENSOR_NDIM['dof_target_velocity'] + 1}" 939 | ) 940 | self._dof_target_velocity = value 941 | 942 | @property 943 | def dof_force(self): 944 | """ """ 945 | return self._dof_force 946 | 947 | @dof_force.setter 948 | def dof_force(self, value): 949 | """ """ 950 | if value is not None: 951 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 952 | if value.ndim not in ( 953 | self._ATTR_TENSOR_NDIM["dof_force"], 954 | self._ATTR_TENSOR_NDIM["dof_force"] + 1, 955 | ): 956 | raise ValueError( 957 | "'dof_force' must have a number of dimensions of " 958 | f"{self._ATTR_TENSOR_NDIM['dof_force']} or " 959 | f"{self._ATTR_TENSOR_NDIM['dof_force'] + 1}" 960 | ) 961 | self._dof_force = value 962 | 963 | @property 964 | def env_ids_reset_base_state(self): 965 | """ """ 966 | return self._env_ids_reset_base_state 967 | 968 | @env_ids_reset_base_state.setter 969 | def env_ids_reset_base_state(self, value): 970 | """ """ 971 | if value is not None: 972 | value = torch.as_tensor(value, dtype=torch.int64, device=self.device) 973 | if value.ndim != 1: 974 | raise ValueError("'env_ids_reset_base_state' must have a number of dimensions of 1") 975 | self._env_ids_reset_base_state = value 976 | 977 | @property 978 | def env_ids_reset_dof_state(self): 979 | """ """ 980 | return self._env_ids_reset_dof_state 981 | 982 | @env_ids_reset_dof_state.setter 983 | def env_ids_reset_dof_state(self, value): 984 | """ """ 985 | if value is not None: 986 | value = torch.as_tensor(value, dtype=torch.int64, device=self.device) 987 | if value.ndim != 1: 988 | raise ValueError("'env_ids_reset_dof_state' must have a number of dimensions of 1") 989 | self._env_ids_reset_dof_state = value 990 | 991 | @property 992 | def dof_state(self): 993 | """ """ 994 | if self._dof_state is None and self._callback_collect_dof_state is not None: 995 | self._callback_collect_dof_state(self) 996 | return self._dof_state 997 | 998 | @dof_state.setter 999 | def dof_state(self, value): 1000 | """ """ 1001 | if value is not None: 1002 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 1003 | self._dof_state = value 1004 | 1005 | @property 1006 | def link_state(self): 1007 | """ """ 1008 | if self._link_state is None and self._callback_collect_link_state is not None: 1009 | self._callback_collect_link_state(self) 1010 | return self._link_state 1011 | 1012 | @link_state.setter 1013 | def link_state(self, value): 1014 | """ """ 1015 | if value is not None: 1016 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 1017 | self._link_state = value 1018 | 1019 | @property 1020 | def contact_id(self): 1021 | """ """ 1022 | return self._contact_id 1023 | 1024 | @contact_id.setter 1025 | def contact_id(self, value): 1026 | """ """ 1027 | if value is not None: 1028 | value = np.asanyarray(value, dtype=np.int64) 1029 | self._contact_id = value 1030 | 1031 | def get_attr_array(self, attr, idx): 1032 | """ """ 1033 | return self._get_attr(attr, self._ATTR_ARRAY_NDIM[attr], idx) 1034 | 1035 | def get_attr_tensor(self, attr, idx): 1036 | """ """ 1037 | return self._get_attr(attr, self._ATTR_TENSOR_NDIM[attr], idx) 1038 | 1039 | def _get_attr(self, attr, ndim, idx): 1040 | """ """ 1041 | array = getattr(self, attr) 1042 | if array.ndim == ndim: 1043 | return array 1044 | if array.ndim == ndim + 1: 1045 | return array[idx] 1046 | 1047 | def lock_attr_array(self): 1048 | """ """ 1049 | for k in self._attr_array_locked: 1050 | if not self._attr_array_locked[k]: 1051 | self._attr_array_locked[k] = True 1052 | if getattr(self, k) is not None: 1053 | getattr(self, k).flags.writeable = False 1054 | 1055 | def update_attr_array(self, attr, env_ids, value): 1056 | """ """ 1057 | if getattr(self, attr).ndim != self._ATTR_ARRAY_NDIM[attr] + 1: 1058 | raise ValueError( 1059 | f"'{attr}' can only be updated when a per-env specification (ndim: " 1060 | f"{self._ATTR_ARRAY_NDIM[attr] + 1}) is used" 1061 | ) 1062 | if len(env_ids) == 0: 1063 | return 1064 | 1065 | env_ids_np = env_ids.cpu().numpy() 1066 | 1067 | with self._make_attr_array_writeable(attr): 1068 | getattr(self, attr)[env_ids_np] = value 1069 | 1070 | if not self._attr_array_dirty_flag[attr]: 1071 | self._attr_array_dirty_flag[attr] = True 1072 | try: 1073 | self._attr_array_dirty_mask[attr][env_ids_np] = True 1074 | except KeyError: 1075 | self._attr_array_dirty_mask[attr] = np.zeros(len(getattr(self, attr)), dtype=bool) 1076 | self._attr_array_dirty_mask[attr][env_ids_np] = True 1077 | 1078 | if self._attr_array_default_flag[attr]: 1079 | self._attr_array_default_flag[attr] = False 1080 | 1081 | @contextmanager 1082 | def _make_attr_array_writeable(self, attr): 1083 | """ """ 1084 | try: 1085 | getattr(self, attr).flags.writeable = True 1086 | yield 1087 | finally: 1088 | getattr(self, attr).flags.writeable = False 1089 | 1090 | def set_callback_collect_dof_state(self, callback): 1091 | """ """ 1092 | self._callback_collect_dof_state = callback 1093 | 1094 | def set_callback_collect_link_state(self, callback): 1095 | """ """ 1096 | self._callback_collect_link_state = callback 1097 | -------------------------------------------------------------------------------- /src/easysim/camera.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import numpy as np 6 | import torch 7 | 8 | from contextlib import contextmanager 9 | 10 | 11 | class Camera: 12 | """ """ 13 | 14 | _ATTR_ARRAY_NDIM = { 15 | "width": 0, 16 | "height": 0, 17 | "vertical_fov": 0, 18 | "near": 0, 19 | "far": 0, 20 | "position": 1, 21 | "target": 1, 22 | "up_vector": 1, 23 | "orientation": 1, 24 | } 25 | 26 | _created = False 27 | 28 | def __init__( 29 | self, 30 | name=None, 31 | device=None, 32 | width=None, 33 | height=None, 34 | vertical_fov=None, 35 | near=None, 36 | far=None, 37 | position=None, 38 | target=None, 39 | up_vector=None, 40 | orientation=None, 41 | ): 42 | """ """ 43 | self._init_attr_array_pipeline() 44 | self._init_callback() 45 | 46 | self.name = name 47 | self.device = device 48 | 49 | self.width = width 50 | self.height = height 51 | self.vertical_fov = vertical_fov 52 | self.near = near 53 | self.far = far 54 | 55 | self.position = position 56 | self.target = target 57 | self.up_vector = up_vector 58 | self.orientation = orientation 59 | 60 | self.color = None 61 | self.depth = None 62 | self.segmentation = None 63 | 64 | self._created = True 65 | 66 | def __setattr__(self, key, value): 67 | """ """ 68 | # Exclude `color`, `depth`, and `segmentation` to prevent infinite recursion in property 69 | # calls. 70 | if ( 71 | self._created 72 | and key not in ("color", "depth", "segmentation") 73 | and not hasattr(self, key) 74 | ): 75 | raise TypeError(f"Unrecognized Body attribute '{key}': {self.name}") 76 | object.__setattr__(self, key, value) 77 | 78 | def _init_attr_array_pipeline(self): 79 | """ """ 80 | self._attr_array_locked = {} 81 | self._attr_array_dirty_flag = {} 82 | self._attr_array_dirty_mask = {} 83 | for attr in self._ATTR_ARRAY_NDIM: 84 | self._attr_array_locked[attr] = False 85 | self._attr_array_dirty_flag[attr] = False 86 | 87 | @property 88 | def attr_array_locked(self): 89 | """ """ 90 | return self._attr_array_locked 91 | 92 | @property 93 | def attr_array_dirty_flag(self): 94 | """ """ 95 | return self._attr_array_dirty_flag 96 | 97 | @property 98 | def attr_array_dirty_mask(self): 99 | """ """ 100 | return self._attr_array_dirty_mask 101 | 102 | def _init_callback(self): 103 | """ """ 104 | self._callback_render_color = None 105 | self._callback_render_depth = None 106 | self._callback_render_segmentation = None 107 | 108 | @property 109 | def name(self): 110 | """ """ 111 | return self._name 112 | 113 | @name.setter 114 | def name(self, value): 115 | """ """ 116 | self._name = value 117 | 118 | @property 119 | def device(self): 120 | """ """ 121 | return self._device 122 | 123 | @device.setter 124 | def device(self, value): 125 | """ """ 126 | self._device = value 127 | 128 | if hasattr(self, "_color") and self.color is not None: 129 | self.color = self.color.to(value) 130 | if hasattr(self, "_depth") and self.depth is not None: 131 | self.depth = self.depth.to(value) 132 | if hasattr(self, "_segmentation") and self.segmentation is not None: 133 | self.segmentation = self.segmentation.to(value) 134 | 135 | @property 136 | def width(self): 137 | """ """ 138 | return self._width 139 | 140 | @width.setter 141 | def width(self, value): 142 | """ """ 143 | assert not self._attr_array_locked[ 144 | "width" 145 | ], "'width' cannot be directly changed after simulation starts. Use 'update_attr_array()'." 146 | if value is not None: 147 | value = np.asanyarray(value, dtype=np.int64) 148 | if value.ndim not in ( 149 | self._ATTR_ARRAY_NDIM["width"], 150 | self._ATTR_ARRAY_NDIM["width"] + 1, 151 | ): 152 | raise ValueError( 153 | "'width' must have a number of dimensions of " 154 | f"{self._ATTR_ARRAY_NDIM['width']} or {self._ATTR_ARRAY_NDIM['width'] + 1}" 155 | ) 156 | self._width = value 157 | 158 | @property 159 | def height(self): 160 | """ """ 161 | return self._height 162 | 163 | @height.setter 164 | def height(self, value): 165 | """ """ 166 | assert not self._attr_array_locked[ 167 | "height" 168 | ], "'height' cannot be directly changed after simulation starts. Use 'update_attr_array()'." 169 | if value is not None: 170 | value = np.asanyarray(value, dtype=np.int64) 171 | if value.ndim not in ( 172 | self._ATTR_ARRAY_NDIM["height"], 173 | self._ATTR_ARRAY_NDIM["height"] + 1, 174 | ): 175 | raise ValueError( 176 | "'height' must have a number of dimensions of " 177 | f"{self._ATTR_ARRAY_NDIM['height']} or {self._ATTR_ARRAY_NDIM['height'] + 1}" 178 | ) 179 | self._height = value 180 | 181 | @property 182 | def vertical_fov(self): 183 | """ """ 184 | return self._vertical_fov 185 | 186 | @vertical_fov.setter 187 | def vertical_fov(self, value): 188 | """ """ 189 | assert not self._attr_array_locked["vertical_fov"], ( 190 | "'vertical_fov' cannot be directly changed after simulation starts. Use " 191 | "'update_attr_array()'." 192 | ) 193 | if value is not None: 194 | value = np.asanyarray(value, dtype=np.float32) 195 | if value.ndim not in ( 196 | self._ATTR_ARRAY_NDIM["vertical_fov"], 197 | self._ATTR_ARRAY_NDIM["vertical_fov"] + 1, 198 | ): 199 | raise ValueError( 200 | "'vertical_fov' must have a number of dimensions of " 201 | f"{self._ATTR_ARRAY_NDIM['vertical_fov']} or " 202 | f"{self._ATTR_ARRAY_NDIM['vertical_fov'] + 1}" 203 | ) 204 | self._vertical_fov = value 205 | 206 | @property 207 | def near(self): 208 | """ """ 209 | return self._near 210 | 211 | @near.setter 212 | def near(self, value): 213 | """ """ 214 | assert not self._attr_array_locked[ 215 | "near" 216 | ], "'near' cannot be directly changed after simulation starts. Use 'update_attr_array()'." 217 | if value is not None: 218 | value = np.asanyarray(value, dtype=np.float32) 219 | if value.ndim not in (self._ATTR_ARRAY_NDIM["near"], self._ATTR_ARRAY_NDIM["near"] + 1): 220 | raise ValueError( 221 | f"'near' must have a number of dimensions of {self._ATTR_ARRAY_NDIM['near']} " 222 | f"or {self._ATTR_ARRAY_NDIM['near'] + 1}" 223 | ) 224 | self._near = value 225 | 226 | @property 227 | def far(self): 228 | """ """ 229 | return self._far 230 | 231 | @far.setter 232 | def far(self, value): 233 | """ """ 234 | assert not self._attr_array_locked[ 235 | "far" 236 | ], "'far' cannot be directly changed after simulation starts. Use 'update_attr_array()'." 237 | if value is not None: 238 | value = np.asanyarray(value, dtype=np.float32) 239 | if value.ndim not in (self._ATTR_ARRAY_NDIM["far"], self._ATTR_ARRAY_NDIM["far"] + 1): 240 | raise ValueError( 241 | f"'far' must have a number of dimensions of {self._ATTR_ARRAY_NDIM['far']} " 242 | f"or {self._ATTR_ARRAY_NDIM['far'] + 1}" 243 | ) 244 | self._far = value 245 | 246 | @property 247 | def position(self): 248 | """ """ 249 | return self._position 250 | 251 | @position.setter 252 | def position(self, value): 253 | """ """ 254 | assert not self._attr_array_locked["position"], ( 255 | "'position' cannot be directly changed after simulation starts. Use " 256 | "'update_attr_array()'." 257 | ) 258 | if value is not None: 259 | value = np.asanyarray(value, dtype=np.float32) 260 | if value.ndim not in ( 261 | self._ATTR_ARRAY_NDIM["position"], 262 | self._ATTR_ARRAY_NDIM["position"] + 1, 263 | ): 264 | raise ValueError( 265 | "'position' must have a number of dimensions of " 266 | f"{self._ATTR_ARRAY_NDIM['position']} or " 267 | f"{self._ATTR_ARRAY_NDIM['position'] + 1}" 268 | ) 269 | if value.shape[-1] != 3: 270 | raise ValueError("'position' must have the last dimension of size 3") 271 | self._position = value 272 | 273 | @property 274 | def target(self): 275 | """ """ 276 | return self._target 277 | 278 | @target.setter 279 | def target(self, value): 280 | """ """ 281 | assert not self._attr_array_locked[ 282 | "target" 283 | ], "'target' cannot be directly changed after simulation starts. Use 'update_attr_array()'." 284 | if value is not None: 285 | if self.orientation is not None: 286 | raise ValueError( 287 | "('target', 'up_vector') and 'orientation' cannot both be set at the same time" 288 | ) 289 | value = np.asanyarray(value, dtype=np.float32) 290 | if value.ndim not in ( 291 | self._ATTR_ARRAY_NDIM["target"], 292 | self._ATTR_ARRAY_NDIM["target"] + 1, 293 | ): 294 | raise ValueError( 295 | "'target' must have a number of dimensions of " 296 | f"{self._ATTR_ARRAY_NDIM['target']} or {self._ATTR_ARRAY_NDIM['target'] + 1}" 297 | ) 298 | if value.shape[-1] != 3: 299 | raise ValueError("'target' must have the last dimension of size 3") 300 | self._target = value 301 | 302 | @property 303 | def up_vector(self): 304 | """ """ 305 | return self._up_vector 306 | 307 | @up_vector.setter 308 | def up_vector(self, value): 309 | """ """ 310 | assert not self._attr_array_locked["up_vector"], ( 311 | "'up_vector' cannot be directly changed after simulation starts. Use " 312 | "'update_attr_array()'." 313 | ) 314 | if value is not None: 315 | if self.orientation is not None: 316 | raise ValueError( 317 | "('target', 'up_vector') and 'orientation' cannot both be set at the same time" 318 | ) 319 | value = np.asanyarray(value, dtype=np.float32) 320 | if value.ndim not in ( 321 | self._ATTR_ARRAY_NDIM["up_vector"], 322 | self._ATTR_ARRAY_NDIM["up_vector"] + 1, 323 | ): 324 | raise ValueError( 325 | "'up_vector' must have a number of dimensions of " 326 | f"{self._ATTR_ARRAY_NDIM['up_vector']} or " 327 | f"{self._ATTR_ARRAY_NDIM['up_vector'] + 1}" 328 | ) 329 | if value.shape[-1] != 3: 330 | raise ValueError("'up_vector' must have the last dimension of size 3") 331 | self._up_vector = value 332 | 333 | @property 334 | def orientation(self): 335 | """ """ 336 | return self._orientation 337 | 338 | @orientation.setter 339 | def orientation(self, value): 340 | """ """ 341 | assert not self._attr_array_locked["orientation"], ( 342 | "'orientation' cannot be directly changed after simulation starts. Use " 343 | "'update_attr_array()'." 344 | ) 345 | if value is not None: 346 | if self.target is not None or self.up_vector is not None: 347 | raise ValueError( 348 | "('target', 'up_vector') and 'orientation' cannot both be set at the same time" 349 | ) 350 | value = np.asanyarray(value, dtype=np.float32) 351 | if value.ndim not in ( 352 | self._ATTR_ARRAY_NDIM["orientation"], 353 | self._ATTR_ARRAY_NDIM["orientation"] + 1, 354 | ): 355 | raise ValueError( 356 | "'orientation' must have a number of dimensions of " 357 | f"{self._ATTR_ARRAY_NDIM['orientation']} or " 358 | f"{self._ATTR_ARRAY_NDIM['orientation'] + 1}" 359 | ) 360 | if value.shape[-1] != 4: 361 | raise ValueError("'orientation' must have the last dimension of size 4") 362 | self._orientation = value 363 | 364 | @property 365 | def color(self): 366 | """ """ 367 | if self._color is None and self._callback_render_color is not None: 368 | self._callback_render_color(self) 369 | return self._color 370 | 371 | @color.setter 372 | def color(self, value): 373 | """ """ 374 | if value is not None: 375 | value = torch.as_tensor(value, dtype=torch.uint8, device=self.device) 376 | self._color = value 377 | 378 | @property 379 | def depth(self): 380 | """ """ 381 | if self._depth is None and self._callback_render_depth is not None: 382 | self._callback_render_depth(self) 383 | return self._depth 384 | 385 | @depth.setter 386 | def depth(self, value): 387 | """ """ 388 | if value is not None: 389 | value = torch.as_tensor(value, dtype=torch.float32, device=self.device) 390 | self._depth = value 391 | 392 | @property 393 | def segmentation(self): 394 | """ """ 395 | if self._segmentation is None and self._callback_render_segmentation is not None: 396 | self._callback_render_segmentation(self) 397 | return self._segmentation 398 | 399 | @segmentation.setter 400 | def segmentation(self, value): 401 | """ """ 402 | if value is not None: 403 | value = torch.as_tensor(value, dtype=torch.int32, device=self.device) 404 | self._segmentation = value 405 | 406 | def get_attr_array(self, attr, idx): 407 | """ """ 408 | return self._get_attr(attr, self._ATTR_ARRAY_NDIM[attr], idx) 409 | 410 | def _get_attr(self, attr, ndim, idx): 411 | """ """ 412 | array = getattr(self, attr) 413 | if array.ndim == ndim: 414 | return array 415 | if array.ndim == ndim + 1: 416 | return array[idx] 417 | 418 | def lock_attr_array(self): 419 | """ """ 420 | for k in self._attr_array_locked: 421 | if not self._attr_array_locked[k]: 422 | self._attr_array_locked[k] = True 423 | if getattr(self, k) is not None: 424 | getattr(self, k).flags.writeable = False 425 | 426 | def update_attr_array(self, attr, env_ids, value): 427 | """ """ 428 | if getattr(self, attr).ndim != self._ATTR_ARRAY_NDIM[attr] + 1: 429 | raise ValueError( 430 | f"'{attr}' can only be updated when a per-env specification (ndim: " 431 | f"{self._ATTR_ARRAY_NDIM[attr] + 1}) is used" 432 | ) 433 | if len(env_ids) == 0: 434 | return 435 | 436 | env_ids_np = env_ids.cpu().numpy() 437 | 438 | with self._make_attr_array_writeable(attr): 439 | getattr(self, attr)[env_ids_np] = value 440 | 441 | if not self._attr_array_dirty_flag[attr]: 442 | self._attr_array_dirty_flag[attr] = True 443 | try: 444 | self._attr_array_dirty_mask[attr][env_ids_np] = True 445 | except KeyError: 446 | self._attr_array_dirty_mask[attr] = np.zeros(len(getattr(self, attr)), dtype=bool) 447 | self._attr_array_dirty_mask[attr][env_ids_np] = True 448 | 449 | @contextmanager 450 | def _make_attr_array_writeable(self, attr): 451 | """ """ 452 | try: 453 | getattr(self, attr).flags.writeable = True 454 | yield 455 | finally: 456 | getattr(self, attr).flags.writeable = False 457 | 458 | def set_callback_render_color(self, callback): 459 | """ """ 460 | self._callback_render_color = callback 461 | 462 | def set_callback_render_depth(self, callback): 463 | """ """ 464 | self._callback_render_depth = callback 465 | 466 | def set_callback_render_segmentation(self, callback): 467 | """ """ 468 | self._callback_render_segmentation = callback 469 | -------------------------------------------------------------------------------- /src/easysim/cmd.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import argparse 6 | 7 | from easysim.config import get_cfg 8 | 9 | 10 | def parse_config_args(): 11 | """ """ 12 | parser = argparse.ArgumentParser() 13 | parser.add_argument("--cfg-file", help="path to config file") 14 | parser.add_argument( 15 | "opts", 16 | nargs=argparse.REMAINDER, 17 | help=( 18 | """modify config options at the end of the command; use space-separated """ 19 | """"PATH.KEY VALUE" pairs; see src/easysim/config.py for all options""" 20 | ), 21 | ) 22 | args = parser.parse_args() 23 | return args 24 | 25 | 26 | def get_config_from_args(): 27 | """ """ 28 | args = parse_config_args() 29 | cfg = get_cfg() 30 | if args.cfg_file is not None: 31 | cfg.merge_from_file(args.cfg_file) 32 | cfg.merge_from_list(args.opts) 33 | return cfg 34 | -------------------------------------------------------------------------------- /src/easysim/config.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | """ 6 | 7 | Derived from: 8 | https://github.com/facebookresearch/detectron2/blob/6e7def97f723bedd25ad6d2aa788802bf982c72c/detectron2/config/defaults.py 9 | """ 10 | 11 | from yacs.config import CfgNode as CN 12 | 13 | 14 | _C = CN() 15 | 16 | # ---------------------------------------------------------------------------- # 17 | # Simulation config 18 | # ---------------------------------------------------------------------------- # 19 | _C.SIM = CN() 20 | 21 | # Simulator choice 22 | # Valid options: ("bullet", "isaac_gym") 23 | _C.SIM.SIMULATOR = "bullet" 24 | 25 | _C.SIM.RENDER = False 26 | 27 | _C.SIM.GRAVITY = (0.0, 0.0, -9.8) 28 | 29 | _C.SIM.USE_DEFAULT_STEP_PARAMS = True 30 | 31 | _C.SIM.TIME_STEP = 1 / 240 32 | 33 | _C.SIM.SUBSTEPS = 1 34 | 35 | _C.SIM.INIT_VIEWER_CAMERA_POSITION = (None, None, None) 36 | 37 | _C.SIM.INIT_VIEWER_CAMERA_TARGET = (None, None, None) 38 | 39 | _C.SIM.DRAW_VIEWER_AXES = True 40 | 41 | _C.SIM.NUM_ENVS = 1 42 | 43 | _C.SIM.SIM_DEVICE = "cpu" 44 | 45 | # ---------------------------------------------------------------------------- # 46 | # Ground plane config 47 | # ---------------------------------------------------------------------------- # 48 | _C.SIM.GROUND_PLANE = CN() 49 | 50 | _C.SIM.GROUND_PLANE.LOAD = True 51 | 52 | _C.SIM.GROUND_PLANE.DISTANCE = 0.0 53 | 54 | # ---------------------------------------------------------------------------- # 55 | # Bullet config 56 | # ---------------------------------------------------------------------------- # 57 | _C.SIM.BULLET = CN() 58 | 59 | _C.SIM.BULLET.USE_EGL = False 60 | 61 | # ---------------------------------------------------------------------------- # 62 | # Isaac Gym config 63 | # ---------------------------------------------------------------------------- # 64 | _C.SIM.ISAAC_GYM = CN() 65 | 66 | _C.SIM.ISAAC_GYM.GRAPHICS_DEVICE_ID = 0 67 | 68 | _C.SIM.ISAAC_GYM.USE_GPU_PIPELINE = False 69 | 70 | _C.SIM.ISAAC_GYM.SPACING = 2.0 71 | 72 | _C.SIM.ISAAC_GYM.RENDER_FRAME_RATE = 60 73 | 74 | _C.SIM.ISAAC_GYM.ENABLE_CAMERA_SENSORS = False 75 | 76 | # ---------------------------------------------------------------------------- # 77 | # Isaac Gym PhysX config 78 | # ---------------------------------------------------------------------------- # 79 | _C.SIM.ISAAC_GYM.PHYSX = CN() 80 | 81 | _C.SIM.ISAAC_GYM.PHYSX.MAX_DEPENETRATION_VELOCITY = 100.0 82 | 83 | # Contact collection mode 84 | # 0: Don't collect any contacts. 85 | # 1: Collect contacts for last substep only. 86 | # 2: Collect contacts for all substeps. 87 | _C.SIM.ISAAC_GYM.PHYSX.CONTACT_COLLECTION = 2 88 | 89 | 90 | cfg = _C 91 | 92 | 93 | def get_cfg(): 94 | """ """ 95 | return _C.clone() 96 | -------------------------------------------------------------------------------- /src/easysim/constants.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | 6 | class GeometryType: 7 | """ """ 8 | 9 | URDF = 0 10 | SPHERE = 1 11 | BOX = 2 12 | 13 | 14 | class DoFControlMode: 15 | """ """ 16 | 17 | NONE = 0 18 | POSITION_CONTROL = 1 19 | VELOCITY_CONTROL = 2 20 | TORQUE_CONTROL = 3 21 | 22 | 23 | class MeshNormalMode: 24 | """ """ 25 | 26 | FROM_ASSET = 0 27 | COMPUTE_PER_VERTEX = 1 28 | COMPUTE_PER_FACE = 2 29 | -------------------------------------------------------------------------------- /src/easysim/contact.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import numpy as np 6 | 7 | _DTYPE = np.dtype( 8 | { 9 | "names": [ 10 | "body_id_a", 11 | "body_id_b", 12 | "link_id_a", 13 | "link_id_b", 14 | "position_a_world", 15 | "position_b_world", 16 | "position_a_link", 17 | "position_b_link", 18 | "normal", 19 | "force", 20 | ], 21 | "formats": [ 22 | np.int32, 23 | np.int32, 24 | np.int32, 25 | np.int32, 26 | [("x", np.float32), ("y", np.float32), ("z", np.float32)], 27 | [("x", np.float32), ("y", np.float32), ("z", np.float32)], 28 | [("x", np.float32), ("y", np.float32), ("z", np.float32)], 29 | [("x", np.float32), ("y", np.float32), ("z", np.float32)], 30 | [("x", np.float32), ("y", np.float32), ("z", np.float32)], 31 | np.float32, 32 | ], 33 | "offsets": [0, 4, 8, 12, 16, 28, 40, 52, 64, 76], 34 | "itemsize": 80, 35 | } 36 | ) 37 | 38 | 39 | def create_contact_array( 40 | length, 41 | body_id_a=None, 42 | body_id_b=None, 43 | link_id_a=None, 44 | link_id_b=None, 45 | position_a_world=None, 46 | position_b_world=None, 47 | position_a_link=None, 48 | position_b_link=None, 49 | normal=None, 50 | force=None, 51 | ): 52 | """ """ 53 | array = np.empty(length, dtype=_DTYPE) 54 | if length > 0: 55 | array["body_id_a"] = body_id_a 56 | array["body_id_b"] = body_id_b 57 | array["link_id_a"] = link_id_a 58 | array["link_id_b"] = link_id_b 59 | array["position_a_world"] = position_a_world 60 | array["position_b_world"] = position_b_world 61 | array["position_a_link"] = position_a_link 62 | array["position_b_link"] = position_b_link 63 | array["normal"] = normal 64 | array["force"] = force 65 | return array 66 | -------------------------------------------------------------------------------- /src/easysim/py.typed: -------------------------------------------------------------------------------- 1 | # Marker file for PEP 561 (https://www.python.org/dev/peps/pep-0561/) stating that this package uses inline types. 2 | -------------------------------------------------------------------------------- /src/easysim/scene.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | from easysim.body import Body 6 | from easysim.camera import Camera 7 | 8 | 9 | class Scene: 10 | """ """ 11 | 12 | def __init__(self): 13 | # A `list` is adopted here rather than a `set`, because for some simulators, the simulation 14 | # outcome is variant to the loading order of bodies. Using a `list` can enforce a 15 | # deterministic and controllable order. 16 | self._bodies = list() 17 | 18 | self._name_to_body = {} 19 | 20 | self._cameras = set() 21 | 22 | self._name_to_camera = {} 23 | 24 | self._init_callback() 25 | 26 | @property 27 | def bodies(self): 28 | """ """ 29 | return self._bodies 30 | 31 | @property 32 | def cameras(self): 33 | """ """ 34 | return self._cameras 35 | 36 | def _init_callback(self): 37 | """ """ 38 | self._callback_add_camera = None 39 | self._callback_remove_camera = None 40 | 41 | def add_body(self, body): 42 | """ """ 43 | if not isinstance(body, Body): 44 | raise TypeError("body must be a Body") 45 | if body in self.bodies: 46 | raise ValueError("body already in scene") 47 | if body.name is None: 48 | raise ValueError("body.name must not be None") 49 | if body.name in self._name_to_body: 50 | raise ValueError(f"Cannot add body with duplicated name: '{body.name}'") 51 | self.bodies.append(body) 52 | 53 | self._name_to_body[body.name] = body 54 | 55 | def remove_body(self, body): 56 | """ """ 57 | if body not in self.bodies: 58 | raise ValueError("body not in the scene") 59 | self.bodies.remove(body) 60 | 61 | del self._name_to_body[body.name] 62 | 63 | def get_body(self, name): 64 | """ """ 65 | if name not in self._name_to_body: 66 | raise ValueError(f"Non-existent body name: '{name}'") 67 | return self._name_to_body[name] 68 | 69 | def add_camera(self, camera): 70 | """ """ 71 | if not isinstance(camera, Camera): 72 | raise TypeError("camera must be a Camera") 73 | if camera in self.cameras: 74 | raise ValueError("camera already in scene") 75 | if camera.name is None: 76 | raise ValueError("camera.name must not be None") 77 | if camera.name in self._name_to_camera: 78 | raise ValueError(f"Cannot add camera with duplicated name: '{camera.name}") 79 | self.cameras.add(camera) 80 | 81 | self._name_to_camera[camera.name] = camera 82 | 83 | if self._callback_add_camera is not None: 84 | self._callback_add_camera(camera) 85 | 86 | def remove_camera(self, camera): 87 | """ """ 88 | if camera not in self.cameras: 89 | raise ValueError("camera not in the scene") 90 | self.cameras.remove(camera) 91 | 92 | del self._name_to_camera[camera.name] 93 | 94 | if self._callback_remove_camera is not None: 95 | self._callback_remove_camera(camera) 96 | 97 | def get_camera(self, name): 98 | """ """ 99 | if name not in self._name_to_camera: 100 | raise ValueError(f"Non-existent camera name: '{name}'") 101 | return self._name_to_camera[name] 102 | 103 | def set_callback_add_camera(self, callback): 104 | """ """ 105 | self._callback_add_camera = callback 106 | 107 | def set_callback_remove_camera(self, callback): 108 | """ """ 109 | self._callback_remove_camera = callback 110 | -------------------------------------------------------------------------------- /src/easysim/simulator_env.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import gym 6 | import abc 7 | 8 | from easysim.scene import Scene 9 | from easysim.simulators.registration import make 10 | 11 | 12 | class SimulatorEnv(gym.Env, abc.ABC): 13 | """ """ 14 | 15 | def __init__(self, cfg, **kwargs): 16 | """ """ 17 | self._cfg = cfg 18 | 19 | self._scene = Scene() 20 | 21 | self.init(**kwargs) 22 | 23 | self._simulator = make(self.cfg.SIM.SIMULATOR, cfg=self.cfg.SIM, scene=self.scene) 24 | 25 | @property 26 | def cfg(self): 27 | """ """ 28 | return self._cfg 29 | 30 | @property 31 | def scene(self): 32 | """ """ 33 | return self._scene 34 | 35 | @property 36 | def simulator(self): 37 | """ """ 38 | return self._simulator 39 | 40 | @abc.abstractmethod 41 | def init(self, **kwargs): 42 | """ """ 43 | 44 | def reset(self, env_ids=None, **kwargs): 45 | """ """ 46 | self.pre_reset(env_ids, **kwargs) 47 | 48 | self._simulator.reset(env_ids) 49 | 50 | observation = self.post_reset(env_ids, **kwargs) 51 | 52 | return observation 53 | 54 | @abc.abstractmethod 55 | def pre_reset(self, env_ids, **kwargs): 56 | """ """ 57 | 58 | @abc.abstractmethod 59 | def post_reset(self, env_ids, **kwargs): 60 | """ """ 61 | 62 | def step(self, action): 63 | """ """ 64 | self.pre_step(action) 65 | 66 | self._simulator.step() 67 | 68 | observation, reward, done, info = self.post_step(action) 69 | 70 | return observation, reward, done, info 71 | 72 | @abc.abstractmethod 73 | def pre_step(self, action): 74 | """ """ 75 | 76 | @abc.abstractmethod 77 | def post_step(self, action): 78 | """ """ 79 | 80 | @property 81 | def contact(self): 82 | """ """ 83 | return self._simulator.contact 84 | 85 | def close(self): 86 | """ """ 87 | self._simulator.close() 88 | 89 | 90 | class SimulatorWrapper(gym.Wrapper): 91 | """ """ 92 | 93 | def reset(self, env_ids=None, **kwargs): 94 | """ """ 95 | return self.env.reset(env_ids=env_ids, **kwargs) 96 | 97 | def step(self, action): 98 | """ """ 99 | return self.env.step(action) 100 | 101 | def close(self): 102 | """ """ 103 | return self.env.close() 104 | -------------------------------------------------------------------------------- /src/easysim/simulators/bullet.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import pybullet 6 | import pybullet_utils.bullet_client as bullet_client 7 | import pybullet_data 8 | import pkgutil 9 | import numpy as np 10 | import time 11 | 12 | from contextlib import contextmanager 13 | 14 | from easysim.simulators.simulator import Simulator 15 | from easysim.constants import GeometryType, DoFControlMode 16 | from easysim.contact import create_contact_array 17 | 18 | 19 | class Bullet(Simulator): 20 | """Bullet simulator.""" 21 | 22 | _ATTR_LINK_DYNAMICS = ( 23 | "link_lateral_friction", 24 | "link_spinning_friction", 25 | "link_rolling_friction", 26 | "link_restitution", 27 | "link_linear_damping", 28 | "link_angular_damping", 29 | ) 30 | _ATTR_DOF_DYNAMICS = ( 31 | "dof_lower_limit", 32 | "dof_upper_limit", 33 | ) 34 | _ATTR_PROJECTION_MATRIX = ("width", "height", "vertical_fov", "near", "far") 35 | _ATTR_VIEW_MATRIX = ("position", "target", "up_vector", "orientation") 36 | _DOF_CONTROL_MODE_MAP = { 37 | DoFControlMode.POSITION_CONTROL: pybullet.POSITION_CONTROL, 38 | DoFControlMode.VELOCITY_CONTROL: pybullet.VELOCITY_CONTROL, 39 | DoFControlMode.TORQUE_CONTROL: pybullet.TORQUE_CONTROL, 40 | } 41 | 42 | def __init__(self, cfg, scene): 43 | """ """ 44 | super().__init__(cfg, scene) 45 | 46 | if not self._cfg.DRAW_VIEWER_AXES: 47 | raise ValueError("DRAW_VIEWER_AXES must be True for Bullet") 48 | if self._cfg.NUM_ENVS != 1: 49 | raise ValueError("NUM_ENVS must be 1 for Bullet") 50 | if self._cfg.SIM_DEVICE != "cpu": 51 | raise ValueError("SIM_DEVICE must be 'cpu' for Bullet") 52 | 53 | self._connected = False 54 | self._last_frame_time = 0.0 55 | 56 | def reset(self, env_ids): 57 | """ """ 58 | if not self._connected: 59 | if self._cfg.RENDER: 60 | self._p = bullet_client.BulletClient(connection_mode=pybullet.GUI) 61 | else: 62 | self._p = bullet_client.BulletClient(connection_mode=pybullet.DIRECT) 63 | self._p.setAdditionalSearchPath(pybullet_data.getDataPath()) 64 | 65 | self._plugins = {} 66 | 67 | if self._cfg.BULLET.USE_EGL: 68 | if self._cfg.RENDER: 69 | raise ValueError("USE_EGL can only be True when RENDER is set to False") 70 | egl = pkgutil.get_loader("eglRenderer") 71 | self._plugins["egl_renderer"] = self._p.loadPlugin( 72 | egl.get_filename(), "_eglRendererPlugin" 73 | ) 74 | 75 | with self._disable_cov_rendering(): 76 | self._p.resetSimulation() 77 | self._p.setGravity(*self._cfg.GRAVITY) 78 | if self._cfg.USE_DEFAULT_STEP_PARAMS: 79 | sim_params = self._p.getPhysicsEngineParameters() 80 | self._cfg.TIME_STEP = sim_params["fixedTimeStep"] 81 | self._cfg.SUBSTEPS = max(sim_params["numSubSteps"], 1) 82 | else: 83 | self._p.setPhysicsEngineParameter( 84 | fixedTimeStep=self._cfg.TIME_STEP, numSubSteps=self._cfg.SUBSTEPS 85 | ) 86 | self._p.setPhysicsEngineParameter(deterministicOverlappingPairs=1) 87 | 88 | if self._cfg.GROUND_PLANE.LOAD: 89 | self._body_id_ground_plane = self._load_ground_plane() 90 | 91 | self._scene_cache = type(self._scene)() 92 | 93 | self._body_ids = {} 94 | self._dof_indices = {} 95 | self._num_links = {} 96 | 97 | for body in self._scene.bodies: 98 | self._load_body(body) 99 | self._cache_body_and_set_control_and_props(body) 100 | self._set_callback_body(body) 101 | 102 | self._projection_matrix = {} 103 | self._view_matrix = {} 104 | self._image_cache = {} 105 | 106 | for camera in self._scene.cameras: 107 | self._load_camera(camera) 108 | self._set_callback_camera(camera) 109 | 110 | if not self._connected: 111 | self._set_callback_scene() 112 | 113 | if ( 114 | self._cfg.RENDER 115 | and self._cfg.INIT_VIEWER_CAMERA_POSITION != (None, None, None) 116 | and self._cfg.INIT_VIEWER_CAMERA_TARGET != (None, None, None) 117 | ): 118 | self._set_viewer_camera_pose( 119 | self._cfg.INIT_VIEWER_CAMERA_POSITION, self._cfg.INIT_VIEWER_CAMERA_TARGET 120 | ) 121 | 122 | if not self._connected: 123 | self._connected = True 124 | 125 | self._clear_state() 126 | self._clear_image() 127 | self._contact = None 128 | 129 | @contextmanager 130 | def _disable_cov_rendering(self): 131 | """ """ 132 | try: 133 | if self._cfg.RENDER: 134 | self._p.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 0) 135 | yield 136 | finally: 137 | if self._cfg.RENDER: 138 | self._p.configureDebugVisualizer(pybullet.COV_ENABLE_RENDERING, 1) 139 | 140 | def _load_ground_plane(self): 141 | """ """ 142 | kwargs = {} 143 | kwargs["basePosition"] = (0.0, 0.0, -self._cfg.GROUND_PLANE.DISTANCE) 144 | return self._p.loadURDF("plane_implicit.urdf", **kwargs) 145 | 146 | def _load_body(self, body): 147 | """ """ 148 | if body.env_ids_load is not None: 149 | if np.array_equal(body.env_ids_load.cpu(), []): 150 | return 151 | elif not np.array_equal(body.env_ids_load.cpu(), [0]): 152 | raise ValueError( 153 | f"For Bullet, 'env_ids_load' must be either None, [] or [0]: '{body.name}'" 154 | ) 155 | 156 | for attr in ("vhacd_enabled", "vhacd_params", "mesh_normal_mode"): 157 | if getattr(body, attr) is not None: 158 | raise ValueError(f"'{attr}' is not supported in Bullet: '{body.name}'") 159 | kwargs = {} 160 | if body.use_self_collision is not None and body.use_self_collision: 161 | kwargs["flags"] = pybullet.URDF_USE_SELF_COLLISION 162 | if body.geometry_type is None: 163 | raise ValueError(f"For Bullet, 'geometry_type' must not be None: '{body.name}'") 164 | if body.geometry_type not in (GeometryType.URDF, GeometryType.SPHERE, GeometryType.BOX): 165 | raise ValueError( 166 | f"For Bullet, 'geometry_type' only supports URDF, SPHERE, and BOX: '{body.name}'" 167 | ) 168 | if body.geometry_type == GeometryType.URDF: 169 | for attr in ("sphere_radius", "box_half_extent"): 170 | if getattr(body, attr) is not None: 171 | raise ValueError(f"'{attr}' must be None for geometry type URDF: '{body.name}'") 172 | if body.use_fixed_base is not None: 173 | kwargs["useFixedBase"] = body.use_fixed_base 174 | if body.scale is not None: 175 | kwargs["globalScaling"] = body.get_attr_array("scale", 0) 176 | if body.attr_array_dirty_flag["scale"]: 177 | body.attr_array_dirty_flag["scale"] = False 178 | self._body_ids[body.name] = self._p.loadURDF(body.urdf_file, **kwargs) 179 | else: 180 | for attr in ("scale",): 181 | if getattr(body, attr) is not None: 182 | geometry_type = [ 183 | x 184 | for x in dir(GeometryType) 185 | if not x.startswith("__") and getattr(GeometryType, x) == body.geometry_type 186 | ][0] 187 | raise ValueError( 188 | f"For Bullet, '{attr}' is not supported for geometry type {geometry_type}:" 189 | f" '{body.name}'" 190 | ) 191 | kwargs_visual = {} 192 | kwargs_collision = {} 193 | if body.geometry_type == GeometryType.SPHERE: 194 | for attr in ("urdf_file", "box_half_extent"): 195 | if getattr(body, attr) is not None: 196 | raise ValueError( 197 | f"'{attr}' must be None for geometry type SPHERE: '{body.name}'" 198 | ) 199 | if body.sphere_radius is not None: 200 | kwargs_visual["radius"] = body.sphere_radius 201 | kwargs_collision["radius"] = body.sphere_radius 202 | kwargs["baseVisualShapeIndex"] = self._p.createVisualShape( 203 | pybullet.GEOM_SPHERE, **kwargs_visual 204 | ) 205 | kwargs["baseCollisionShapeIndex"] = self._p.createCollisionShape( 206 | pybullet.GEOM_SPHERE, **kwargs_collision 207 | ) 208 | if body.geometry_type == GeometryType.BOX: 209 | for attr in ("urdf_file", "sphere_radius"): 210 | if getattr(body, attr) is not None: 211 | raise ValueError( 212 | f"'{attr}' must be None for geometry type BOX: '{body.name}'" 213 | ) 214 | if body.box_half_extent is not None: 215 | kwargs_visual["halfExtents"] = body.box_half_extent 216 | kwargs_collision["halfExtents"] = body.box_half_extent 217 | kwargs["baseVisualShapeIndex"] = self._p.createVisualShape( 218 | pybullet.GEOM_BOX, **kwargs_visual 219 | ) 220 | kwargs["baseCollisionShapeIndex"] = self._p.createCollisionShape( 221 | pybullet.GEOM_BOX, **kwargs_collision 222 | ) 223 | if body.use_fixed_base is not None and body.use_fixed_base: 224 | kwargs["baseMass"] = 0.0 225 | self._body_ids[body.name] = self._p.createMultiBody(**kwargs) 226 | 227 | dof_indices = [] 228 | for j in range(self._p.getNumJoints(self._body_ids[body.name])): 229 | joint_info = self._p.getJointInfo(self._body_ids[body.name], j) 230 | if joint_info[2] != pybullet.JOINT_FIXED: 231 | dof_indices.append(j) 232 | self._dof_indices[body.name] = np.asanyarray(dof_indices, dtype=np.int64) 233 | 234 | self._num_links[body.name] = self._p.getNumJoints(self._body_ids[body.name]) + 1 235 | 236 | # Reset base state. 237 | if body.initial_base_position is not None: 238 | self._reset_base_position(body) 239 | if body.initial_base_velocity is not None: 240 | self._reset_base_velocity(body) 241 | 242 | # Reset DoF state. 243 | if len(self._dof_indices[body.name]) == 0: 244 | for attr in ("initial_dof_position", "initial_dof_velocity"): 245 | if getattr(body, attr) is not None: 246 | raise ValueError(f"'{attr}' must be None for body with 0 DoF: '{body.name}'") 247 | if body.initial_dof_position is not None: 248 | self._reset_dof_state(body) 249 | elif body.initial_dof_velocity is not None: 250 | raise ValueError( 251 | "For Bullet, cannot reset 'initial_dof_velocity' without resetting " 252 | f"'initial_dof_position': '{body.name}'" 253 | ) 254 | 255 | body.contact_id = [self._body_ids[body.name]] 256 | 257 | def _reset_base_position(self, body): 258 | """ """ 259 | if body.initial_base_position.ndim == 1: 260 | self._p.resetBasePositionAndOrientation( 261 | self._body_ids[body.name], 262 | body.initial_base_position[:3], 263 | body.initial_base_position[3:], 264 | ) 265 | if body.initial_base_position.ndim == 2: 266 | self._p.resetBasePositionAndOrientation( 267 | self._body_ids[body.name], 268 | body.initial_base_position[0, :3], 269 | body.initial_base_position[0, 3:], 270 | ) 271 | 272 | def _reset_base_velocity(self, body): 273 | """ """ 274 | kwargs = {} 275 | if body.initial_base_velocity.ndim == 1: 276 | kwargs["linearVelocity"] = body.initial_base_velocity[:3] 277 | kwargs["angularVelocity"] = body.initial_base_velocity[3:] 278 | if body.initial_base_velocity.ndim == 2: 279 | kwargs["linearVelocity"] = body.initial_base_velocity[0, :3] 280 | kwargs["angularVelocity"] = body.initial_base_velocity[0, 3:] 281 | self._p.resetBaseVelocity(self._body_ids[body.name], **kwargs) 282 | 283 | def _reset_dof_state(self, body): 284 | """ """ 285 | for i, j in enumerate(self._dof_indices[body.name]): 286 | kwargs = {} 287 | if body.initial_dof_velocity is not None: 288 | if body.initial_dof_velocity.ndim == 1: 289 | kwargs["targetVelocity"] = body.initial_dof_velocity[i] 290 | if body.initial_dof_velocity.ndim == 2: 291 | kwargs["targetVelocity"] = body.initial_dof_velocity[0, i] 292 | if body.initial_dof_position.ndim == 1: 293 | self._p.resetJointState( 294 | self._body_ids[body.name], j, body.initial_dof_position[i], **kwargs 295 | ) 296 | if body.initial_dof_position.ndim == 2: 297 | self._p.resetJointState( 298 | self._body_ids[body.name], j, body.initial_dof_position[0, i], **kwargs 299 | ) 300 | 301 | def _cache_body_and_set_control_and_props(self, body): 302 | """ """ 303 | x = type(body)() 304 | x.name = body.name 305 | self._scene_cache.add_body(x) 306 | 307 | if body.env_ids_load is not None and len(body.env_ids_load) == 0: 308 | body.lock_attr_array() 309 | return 310 | 311 | for attr in ("link_segmentation_id", "dof_has_limits", "dof_armature"): 312 | if getattr(body, attr) is not None: 313 | raise ValueError(f"'{attr}' is not supported in Bullet: '{body.name}'") 314 | 315 | if len(self._dof_indices[body.name]) == 0: 316 | for attr in ("dof_lower_limit", "dof_upper_limit", "dof_control_mode"): 317 | if getattr(body, attr) is not None: 318 | raise ValueError(f"'{attr}' must be None for body with 0 DoF: '{body.name}'") 319 | 320 | if body.dof_control_mode is not None: 321 | if ( 322 | body.dof_control_mode.ndim == 0 323 | and body.dof_control_mode 324 | not in ( 325 | DoFControlMode.POSITION_CONTROL, 326 | DoFControlMode.VELOCITY_CONTROL, 327 | DoFControlMode.TORQUE_CONTROL, 328 | ) 329 | or body.dof_control_mode.ndim == 1 330 | and any( 331 | y 332 | not in ( 333 | DoFControlMode.POSITION_CONTROL, 334 | DoFControlMode.VELOCITY_CONTROL, 335 | DoFControlMode.TORQUE_CONTROL, 336 | ) 337 | for y in body.dof_control_mode 338 | ) 339 | ): 340 | raise ValueError( 341 | "For Bullet, 'dof_control_mode' only supports POSITION_CONTROL, " 342 | f"VELOCITY_CONTROL, and TORQUE_CONTROL: '{body.name}'" 343 | ) 344 | 345 | if ( 346 | body.dof_control_mode.ndim == 0 347 | and body.dof_control_mode == DoFControlMode.TORQUE_CONTROL 348 | ): 349 | self._p.setJointMotorControlArray( 350 | self._body_ids[body.name], 351 | self._dof_indices[body.name], 352 | pybullet.VELOCITY_CONTROL, 353 | forces=[0] * len(self._dof_indices[body.name]), 354 | ) 355 | if ( 356 | body.dof_control_mode.ndim == 1 357 | and DoFControlMode.TORQUE_CONTROL in body.dof_control_mode 358 | ): 359 | self._p.setJointMotorControlArray( 360 | self._body_ids[body.name], 361 | self._dof_indices[body.name][ 362 | body.dof_control_mode == DoFControlMode.TORQUE_CONTROL 363 | ], 364 | pybullet.VELOCITY_CONTROL, 365 | forces=[0] 366 | * len( 367 | self._dof_indices[body.name][ 368 | body.dof_control_mode == DoFControlMode.TORQUE_CONTROL 369 | ] 370 | ), 371 | ) 372 | elif len(self._dof_indices[body.name]) > 0: 373 | raise ValueError( 374 | f"For Bullet, 'dof_control_mode' is required for body with DoF > 0: '{body.name}'" 375 | ) 376 | 377 | if body.link_collision_filter is not None: 378 | self._set_link_collision_filter(body) 379 | 380 | if any( 381 | not body.attr_array_default_flag[x] and getattr(body, x) is not None 382 | for x in self._ATTR_LINK_DYNAMICS 383 | ): 384 | self._set_link_dynamics(body) 385 | 386 | if not body.attr_array_default_flag["link_color"] and body.link_color is not None: 387 | self._set_link_color(body) 388 | 389 | if len(self._dof_indices[body.name]) > 0 and any( 390 | not body.attr_array_default_flag[x] and getattr(body, x) is not None 391 | for x in self._ATTR_DOF_DYNAMICS 392 | ): 393 | self._set_dof_dynamics(body) 394 | 395 | if any( 396 | getattr(body, x) is None 397 | for x in self._ATTR_LINK_DYNAMICS 398 | if x not in ("link_linear_damping", "link_angular_damping") 399 | ): 400 | dynamics_info = [ 401 | self._p.getDynamicsInfo(self._body_ids[body.name], i) 402 | for i in range(-1, self._num_links[body.name] - 1) 403 | ] 404 | if body.link_lateral_friction is None: 405 | body.link_lateral_friction = [[x[1] for x in dynamics_info]] 406 | body.attr_array_default_flag["link_lateral_friction"] = True 407 | if body.link_spinning_friction is None: 408 | body.link_spinning_friction = [[x[7] for x in dynamics_info]] 409 | body.attr_array_default_flag["link_spinning_friction"] = True 410 | if body.link_rolling_friction is None: 411 | body.link_rolling_friction = [[x[6] for x in dynamics_info]] 412 | body.attr_array_default_flag["link_rolling_friction"] = True 413 | if body.link_restitution is None: 414 | body.link_restitution = [[x[5] for x in dynamics_info]] 415 | body.attr_array_default_flag["link_restitution"] = True 416 | 417 | if body.link_color is None: 418 | visual_data = self._p.getVisualShapeData(self._body_ids[body.name]) 419 | body.link_color = [[x[7] for x in visual_data]] 420 | body.attr_array_default_flag["link_color"] = True 421 | 422 | if len(self._dof_indices[body.name]) > 0 and any( 423 | getattr(body, x) is None for x in self._ATTR_DOF_DYNAMICS 424 | ): 425 | joint_info = [ 426 | self._p.getJointInfo(self._body_ids[body.name], j) 427 | for j in self._dof_indices[body.name] 428 | ] 429 | if body.dof_lower_limit is None: 430 | body.dof_lower_limit = [[x[8] for x in joint_info]] 431 | body.attr_array_default_flag["dof_lower_limit"] = True 432 | if body.dof_upper_limit is None: 433 | body.dof_upper_limit = [[x[9] for x in joint_info]] 434 | body.attr_array_default_flag["dof_upper_limit"] = True 435 | 436 | body.lock_attr_array() 437 | 438 | for attr in ( 439 | ("link_color", "link_collision_filter") 440 | + self._ATTR_LINK_DYNAMICS 441 | + self._ATTR_DOF_DYNAMICS 442 | ): 443 | if body.attr_array_dirty_flag[attr]: 444 | body.attr_array_dirty_flag[attr] = False 445 | 446 | def _set_link_collision_filter(self, body): 447 | """ """ 448 | link_collision_filter = body.get_attr_array("link_collision_filter", 0) 449 | if ( 450 | not body.attr_array_locked["link_collision_filter"] 451 | and len(link_collision_filter) != self._num_links[body.name] 452 | ): 453 | raise ValueError( 454 | "Size of 'link_collision_filter' in the link dimension " 455 | f"({len(link_collision_filter)}) should match the number of links " 456 | f"({self._num_links[body.name]}): '{body.name}'" 457 | ) 458 | for i in range(-1, self._num_links[body.name] - 1): 459 | self._p.setCollisionFilterGroupMask( 460 | self._body_ids[body.name], 461 | i, 462 | link_collision_filter[i + 1], 463 | link_collision_filter[i + 1], 464 | ) 465 | 466 | def _set_link_dynamics(self, body, dirty_only=False): 467 | """ """ 468 | for attr in self._ATTR_LINK_DYNAMICS: 469 | if attr in ("link_linear_damping", "link_angular_damping"): 470 | continue 471 | if ( 472 | not body.attr_array_locked[attr] 473 | and getattr(body, attr) is not None 474 | and len(body.get_attr_array(attr, 0)) != self._num_links[body.name] 475 | ): 476 | raise ValueError( 477 | f"Size of '{attr}' in the link dimension ({len(body.get_attr_array(attr, 0))}) " 478 | f"should match the number of links ({self._num_links[body.name]}): " 479 | f"'{body.name}'" 480 | ) 481 | kwargs = {} 482 | if ( 483 | not dirty_only 484 | and not body.attr_array_default_flag["link_lateral_friction"] 485 | and body.link_lateral_friction is not None 486 | or body.attr_array_dirty_flag["link_lateral_friction"] 487 | ): 488 | kwargs["lateralFriction"] = body.get_attr_array("link_lateral_friction", 0) 489 | if ( 490 | not dirty_only 491 | and not body.attr_array_default_flag["link_spinning_friction"] 492 | and body.link_spinning_friction is not None 493 | or body.attr_array_dirty_flag["link_spinning_friction"] 494 | ): 495 | kwargs["spinningFriction"] = body.get_attr_array("link_spinning_friction", 0) 496 | if ( 497 | not dirty_only 498 | and not body.attr_array_default_flag["link_rolling_friction"] 499 | and body.link_rolling_friction is not None 500 | or body.attr_array_dirty_flag["link_rolling_friction"] 501 | ): 502 | kwargs["rollingFriction"] = body.get_attr_array("link_rolling_friction", 0) 503 | if ( 504 | not dirty_only 505 | and not body.attr_array_default_flag["link_restitution"] 506 | and body.link_restitution is not None 507 | or body.attr_array_dirty_flag["link_restitution"] 508 | ): 509 | kwargs["restitution"] = body.get_attr_array("link_restitution", 0) 510 | if len(kwargs) > 0: 511 | for i in range(-1, self._num_links[body.name] - 1): 512 | self._p.changeDynamics( 513 | self._body_ids[body.name], i, **{k: v[i + 1] for k, v in kwargs.items()} 514 | ) 515 | # Bullet only sets `linearDamping` and `angularDamping` for link index -1. See: 516 | # https://github.com/bulletphysics/bullet3/blob/740d2b978352b16943b24594572586d95d476466/examples/SharedMemory/PhysicsClientC_API.cpp#L3419 517 | # https://github.com/bulletphysics/bullet3/blob/740d2b978352b16943b24594572586d95d476466/examples/SharedMemory/PhysicsClientC_API.cpp#L3430 518 | kwargs = {} 519 | if ( 520 | not dirty_only 521 | and not body.attr_array_default_flag["link_linear_damping"] 522 | and body.link_linear_damping is not None 523 | or body.attr_array_dirty_flag["link_linear_damping"] 524 | ): 525 | kwargs["linearDamping"] = body.get_attr_array("link_linear_damping", 0) 526 | if ( 527 | not dirty_only 528 | and not body.attr_array_default_flag["link_angular_damping"] 529 | and body.link_angular_damping is not None 530 | or body.attr_array_dirty_flag["link_angular_damping"] 531 | ): 532 | kwargs["angularDamping"] = body.get_attr_array("link_angular_damping", 0) 533 | if len(kwargs) > 0: 534 | self._p.changeDynamics( 535 | self._body_ids[body.name], -1, **{k: v for k, v in kwargs.items()} 536 | ) 537 | 538 | def _set_link_color(self, body): 539 | """ """ 540 | link_color = body.get_attr_array("link_color", 0) 541 | if ( 542 | not body.attr_array_locked["link_color"] 543 | and len(link_color) != self._num_links[body.name] 544 | ): 545 | raise ValueError( 546 | f"Size of 'link_color' in the link dimension ({len(link_color)}) should match the " 547 | f"number of links: '{body.name}' ({self._num_links[body.name]})" 548 | ) 549 | for i in range(-1, self._num_links[body.name] - 1): 550 | self._p.changeVisualShape(self._body_ids[body.name], i, rgbaColor=link_color[i + 1]) 551 | 552 | def _set_dof_dynamics(self, body, dirty_only=False): 553 | """ """ 554 | for attr in self._ATTR_DOF_DYNAMICS: 555 | if ( 556 | not body.attr_array_locked[attr] 557 | and getattr(body, attr) is not None 558 | and len(body.get_attr_array(attr, 0)) != len(self._dof_indices[body.name]) 559 | ): 560 | raise ValueError( 561 | f"Size of '{attr}' in the DoF dimension ({len(body.get_attr_array(attr, 0))}) " 562 | f"should match the number of DoFs ({len(self._dof_indices[body.name])}): " 563 | f"'{body.name}'" 564 | ) 565 | kwargs = {} 566 | if ( 567 | not dirty_only 568 | and not body.attr_array_default_flag["dof_lower_limit"] 569 | and body.dof_lower_limit is not None 570 | or body.attr_array_dirty_flag["dof_lower_limit"] 571 | ): 572 | kwargs["jointLowerLimit"] = body.get_attr_array("dof_lower_limit", 0) 573 | if ( 574 | not dirty_only 575 | and not body.attr_array_default_flag["dof_upper_limit"] 576 | and body.dof_upper_limit is not None 577 | or body.attr_array_dirty_flag["dof_upper_limit"] 578 | ): 579 | kwargs["jointUpperLimit"] = body.get_attr_array("dof_upper_limit", 0) 580 | if len(kwargs) > 0: 581 | for i, j in enumerate(self._dof_indices[body.name]): 582 | self._p.changeDynamics( 583 | self._body_ids[body.name], j, **{k: v[i] for k, v in kwargs.items()} 584 | ) 585 | 586 | def _set_callback_body(self, body): 587 | """ """ 588 | body.set_callback_collect_dof_state(self._collect_dof_state) 589 | body.set_callback_collect_link_state(self._collect_link_state) 590 | 591 | def _collect_dof_state(self, body): 592 | """ """ 593 | if len(self._dof_indices[body.name]) > 0: 594 | joint_states = self._p.getJointStates( 595 | self._body_ids[body.name], self._dof_indices[body.name] 596 | ) 597 | dof_state = [x[0:2] for x in joint_states] 598 | body.dof_state = [dof_state] 599 | 600 | def _collect_link_state(self, body): 601 | """ """ 602 | pos, orn = self._p.getBasePositionAndOrientation(self._body_ids[body.name]) 603 | lin, ang = self._p.getBaseVelocity(self._body_ids[body.name]) 604 | link_state = [pos + orn + lin + ang] 605 | if self._num_links[body.name] > 1: 606 | link_indices = [*range(0, self._num_links[body.name] - 1)] 607 | # Need to set computeForwardKinematics=1. See: 608 | # https://github.com/bulletphysics/bullet3/issues/2806 609 | link_states = self._p.getLinkStates( 610 | self._body_ids[body.name], 611 | link_indices, 612 | computeLinkVelocity=1, 613 | computeForwardKinematics=1, 614 | ) 615 | link_state += [x[4] + x[5] + x[6] + x[7] for x in link_states] 616 | body.link_state = [link_state] 617 | 618 | def _load_camera(self, camera): 619 | """ """ 620 | self._set_projection_matrix(camera) 621 | self._set_view_matrix(camera) 622 | 623 | self._image_cache[camera.name] = {} 624 | self._clear_image_cache(camera) 625 | 626 | camera.lock_attr_array() 627 | 628 | def _set_projection_matrix(self, camera): 629 | """ """ 630 | self._projection_matrix[camera.name] = self._p.computeProjectionMatrixFOV( 631 | camera.get_attr_array("vertical_fov", 0), 632 | camera.get_attr_array("width", 0) / camera.get_attr_array("height", 0), 633 | camera.get_attr_array("near", 0), 634 | camera.get_attr_array("far", 0), 635 | ) 636 | 637 | def _set_view_matrix(self, camera): 638 | """ """ 639 | if camera.target is not None and camera.up_vector is not None: 640 | self._view_matrix[camera.name] = self._p.computeViewMatrix( 641 | camera.get_attr_array("position", 0), 642 | camera.get_attr_array("target", 0), 643 | camera.get_attr_array("up_vector", 0), 644 | ) 645 | elif camera.orientation is not None: 646 | R = np.array( 647 | self._p.getMatrixFromQuaternion(camera.get_attr_array("orientation", 0)), 648 | dtype=np.float32, 649 | ).reshape(3, 3) 650 | view_matrix = np.eye(4, dtype=np.float32) 651 | view_matrix[:3, :3] = R 652 | view_matrix[3, :3] = -1 * camera.get_attr_array("position", 0).dot(R) 653 | self._view_matrix[camera.name] = tuple(view_matrix.flatten()) 654 | else: 655 | raise ValueError( 656 | "For Bullet, either ('target', 'up_vector') or 'orientation' is required to be " 657 | f"set: {camera.name}" 658 | ) 659 | 660 | def _clear_image_cache(self, camera): 661 | """ """ 662 | self._image_cache[camera.name]["color"] = None 663 | self._image_cache[camera.name]["depth"] = None 664 | self._image_cache[camera.name]["segmentation"] = None 665 | 666 | def _set_callback_camera(self, camera): 667 | """ """ 668 | camera.set_callback_render_color(self._render_color) 669 | camera.set_callback_render_depth(self._render_depth) 670 | camera.set_callback_render_segmentation(self._render_segmentation) 671 | 672 | def _render_color(self, camera): 673 | """ """ 674 | for body in self._scene.bodies: 675 | if body.attr_array_dirty_flag["link_color"]: 676 | self._set_link_color(body) 677 | body.attr_array_dirty_flag["link_color"] = False 678 | if self._image_cache[camera.name]["color"] is not None: 679 | self._image_cache[camera.name]["color"] = None 680 | 681 | self._check_and_update_camera(camera) 682 | if self._image_cache[camera.name]["color"] is None: 683 | self._render(camera) 684 | camera.color = self._image_cache[camera.name]["color"][None] 685 | 686 | def _render_depth(self, camera): 687 | """ """ 688 | self._check_and_update_camera(camera) 689 | if self._image_cache[camera.name]["depth"] is None: 690 | self._render(camera) 691 | depth = ( 692 | camera.get_attr_array("far", 0) 693 | * camera.get_attr_array("near", 0) 694 | / ( 695 | camera.get_attr_array("far", 0) 696 | - (camera.get_attr_array("far", 0) - camera.get_attr_array("near", 0)) 697 | * self._image_cache[camera.name]["depth"] 698 | ) 699 | ) 700 | depth[self._image_cache[camera.name]["depth"] == 1.0] = 0.0 701 | camera.depth = depth[None] 702 | 703 | def _render_segmentation(self, camera): 704 | """ """ 705 | for body in self._scene.bodies: 706 | for attr in ("link_segmentation_id",): 707 | if getattr(body, attr) is not None: 708 | raise ValueError(f"'{attr}' is not supported in Bullet: '{body.name}'") 709 | 710 | self._check_and_update_camera(camera) 711 | if self._image_cache[camera.name]["segmentation"] is None: 712 | self._render(camera) 713 | camera.segmentation = self._image_cache[camera.name]["segmentation"][None] 714 | 715 | def _check_and_update_camera(self, camera): 716 | """ """ 717 | if any(camera.attr_array_dirty_flag[x] for x in self._ATTR_PROJECTION_MATRIX): 718 | self._set_projection_matrix(camera) 719 | for attr in self._ATTR_PROJECTION_MATRIX: 720 | if camera.attr_array_dirty_flag[attr]: 721 | camera.attr_array_dirty_flag[attr] = False 722 | self._clear_image_cache(camera) 723 | if any(camera.attr_array_dirty_flag[x] for x in self._ATTR_VIEW_MATRIX): 724 | self._set_view_matrix(camera) 725 | for attr in self._ATTR_VIEW_MATRIX: 726 | if camera.attr_array_dirty_flag[attr]: 727 | camera.attr_array_dirty_flag[attr] = False 728 | self._clear_image_cache(camera) 729 | 730 | def _render(self, camera): 731 | """ """ 732 | ( 733 | _, 734 | _, 735 | self._image_cache[camera.name]["color"], 736 | self._image_cache[camera.name]["depth"], 737 | self._image_cache[camera.name]["segmentation"], 738 | ) = self._p.getCameraImage( 739 | camera.get_attr_array("width", 0), 740 | camera.get_attr_array("height", 0), 741 | viewMatrix=self._view_matrix[camera.name], 742 | projectionMatrix=self._projection_matrix[camera.name], 743 | renderer=pybullet.ER_BULLET_HARDWARE_OPENGL, 744 | ) 745 | 746 | def _set_callback_scene(self): 747 | """ """ 748 | self._scene.set_callback_add_camera(self._add_camera) 749 | self._scene.set_callback_remove_camera(self._remove_camera) 750 | 751 | def _add_camera(self, camera): 752 | """ """ 753 | self._load_camera(camera) 754 | self._set_callback_camera(camera) 755 | 756 | def _remove_camera(self, camera): 757 | """ """ 758 | del self._projection_matrix[camera.name] 759 | del self._view_matrix[camera.name] 760 | del self._image_cache[camera.name] 761 | 762 | def _set_viewer_camera_pose(self, position, target): 763 | """ """ 764 | disp = [x - y for x, y in zip(position, target)] 765 | dist = np.linalg.norm(disp) 766 | yaw = np.arctan2(disp[0], -disp[1]) 767 | yaw = np.rad2deg(yaw) 768 | pitch = np.arctan2(-disp[2], np.linalg.norm((disp[0], disp[1]))) 769 | pitch = np.rad2deg(pitch) 770 | 771 | self._p.resetDebugVisualizerCamera(dist, yaw, pitch, target) 772 | 773 | def _clear_state(self): 774 | """ """ 775 | for body in self._scene.bodies: 776 | body.dof_state = None 777 | body.link_state = None 778 | 779 | def _clear_image(self): 780 | """ """ 781 | for camera in self._scene.cameras: 782 | camera.color = None 783 | camera.depth = None 784 | camera.segmentation = None 785 | 786 | self._clear_image_cache(camera) 787 | 788 | def step(self): 789 | """ """ 790 | if self._cfg.RENDER: 791 | # Simulate real-time rendering with sleep if computation takes less than real time. 792 | time_spent = time.time() - self._last_frame_time 793 | time_sleep = self._cfg.TIME_STEP - time_spent 794 | if time_sleep > 0: 795 | time.sleep(time_sleep) 796 | self._last_frame_time = time.time() 797 | 798 | for body in reversed(self._scene_cache.bodies): 799 | if body.name not in [x.name for x in self._scene.bodies]: 800 | # Remove body. 801 | with self._disable_cov_rendering(): 802 | self._p.removeBody(self._body_ids[body.name]) 803 | del self._body_ids[body.name] 804 | del self._dof_indices[body.name] 805 | del self._num_links[body.name] 806 | self._scene_cache.remove_body(body) 807 | for body in self._scene.bodies: 808 | if body.name not in [x.name for x in self._scene_cache.bodies]: 809 | # Add body. 810 | with self._disable_cov_rendering(): 811 | self._load_body(body) 812 | self._cache_body_and_set_control_and_props(body) 813 | self._set_callback_body(body) 814 | 815 | for body in self._scene.bodies: 816 | for attr in ( 817 | "link_segmentation_id", 818 | "dof_has_limits", 819 | "dof_armature", 820 | ): 821 | if getattr(body, attr) is not None: 822 | raise ValueError(f"'{attr}' is not supported in Bullet: '{body.name}'") 823 | 824 | if body.env_ids_load is not None and len(body.env_ids_load) == 0: 825 | for attr in ("env_ids_reset_base_state", "env_ids_reset_dof_state"): 826 | if getattr(body, attr) is not None: 827 | raise ValueError( 828 | f"For Bullet, '{attr}' should be None if 'env_ids_load' is set to []: " 829 | f"'{body.name}'" 830 | ) 831 | continue 832 | 833 | if body.env_ids_reset_base_state is not None: 834 | if not np.array_equal(body.env_ids_reset_base_state.cpu(), [0]): 835 | raise ValueError( 836 | "For Bullet, 'env_ids_reset_base_state' must be either None or [0]: " 837 | f"'{body.name}'" 838 | ) 839 | if body.initial_base_position is None and body.initial_base_velocity is None: 840 | raise ValueError( 841 | "'initial_base_position' and 'initial_base_velocity' cannot be both None " 842 | f"when 'env_ids_reset_base_state' is used: {body.name}" 843 | ) 844 | if body.initial_base_position is not None: 845 | self._reset_base_position(body) 846 | if body.initial_base_velocity is not None: 847 | self._reset_base_velocity(body) 848 | body.env_ids_reset_base_state = None 849 | 850 | if body.attr_array_dirty_flag["scale"]: 851 | raise ValueError( 852 | f"For Bullet, 'scale' can only be changed before each reset: '{body.name}'" 853 | ) 854 | if body.attr_array_dirty_flag["link_collision_filter"]: 855 | self._set_link_collision_filter(body) 856 | body.attr_array_dirty_flag["link_collision_filter"] = False 857 | if any(body.attr_array_dirty_flag[x] for x in self._ATTR_LINK_DYNAMICS): 858 | self._set_link_dynamics(body, dirty_only=True) 859 | for attr in self._ATTR_LINK_DYNAMICS: 860 | if body.attr_array_dirty_flag[attr]: 861 | body.attr_array_dirty_flag[attr] = False 862 | if body.attr_array_dirty_flag["link_color"]: 863 | self._set_link_color(body) 864 | body.attr_array_dirty_flag["link_color"] = False 865 | 866 | if len(self._dof_indices[body.name]) == 0: 867 | for attr in ( 868 | "dof_lower_limit", 869 | "dof_upper_limit", 870 | "dof_control_mode", 871 | "dof_max_force", 872 | "dof_max_velocity", 873 | "dof_position_gain", 874 | "dof_velocity_gain", 875 | "dof_target_position", 876 | "dof_target_velocity", 877 | "dof_force", 878 | "env_ids_reset_dof_state", 879 | ): 880 | if getattr(body, attr) is not None: 881 | raise ValueError( 882 | f"'{attr}' must be None for body with 0 DoF: '{body.name}'" 883 | ) 884 | continue 885 | 886 | if body.env_ids_reset_dof_state is not None: 887 | if not np.array_equal(body.env_ids_reset_dof_state.cpu(), [0]): 888 | raise ValueError( 889 | "For Bullet, 'env_ids_reset_dof_state' must be either None or [0]: " 890 | f"'{body.name}'" 891 | ) 892 | self._reset_dof_state(body) 893 | body.env_ids_reset_dof_state = None 894 | 895 | if any(body.attr_array_dirty_flag[x] for x in self._ATTR_DOF_DYNAMICS): 896 | self._set_dof_dynamics(body, dirty_only=True) 897 | for attr in self._ATTR_DOF_DYNAMICS: 898 | if body.attr_array_dirty_flag[attr]: 899 | body.attr_array_dirty_flag[attr] = False 900 | 901 | if body.attr_array_dirty_flag["dof_control_mode"]: 902 | raise ValueError( 903 | "For Bullet, 'dof_control_mode' cannot be changed after each reset: " 904 | f"'{body.name}'" 905 | ) 906 | # The redundant if-else block below is an artifact due to `setJointMotorControlArray()` 907 | # not supporting `maxVelocity`. `setJointMotorControlArray()` is still preferred when 908 | # `maxVelocity` is not needed due to better speed performance. 909 | if body.dof_max_velocity is None: 910 | kwargs = {} 911 | if body.dof_target_position is not None: 912 | kwargs["targetPositions"] = body.get_attr_tensor("dof_target_position", 0) 913 | if body.dof_target_velocity is not None: 914 | kwargs["targetVelocities"] = body.get_attr_tensor("dof_target_velocity", 0) 915 | if body.dof_position_gain is not None: 916 | kwargs["positionGains"] = body.get_attr_array("dof_position_gain", 0) 917 | if body.dof_velocity_gain is not None: 918 | kwargs["velocityGains"] = body.get_attr_array("dof_velocity_gain", 0) 919 | if body.dof_control_mode.ndim == 0: 920 | if body.dof_max_force is not None: 921 | if body.dof_control_mode not in ( 922 | DoFControlMode.POSITION_CONTROL, 923 | DoFControlMode.VELOCITY_CONTROL, 924 | ): 925 | raise ValueError( 926 | "For Bullet, 'dof_max_force' can only be set in POSITION_CONTROL " 927 | f"and VELOCITY_CONTROL modes: '{body.name}'" 928 | ) 929 | kwargs["forces"] = body.get_attr_array("dof_max_force", 0) 930 | if body.dof_force is not None: 931 | if body.dof_control_mode != DoFControlMode.TORQUE_CONTROL: 932 | raise ValueError( 933 | "For Bullet, 'dof_force' can only be set in the TORQUE_CONTROL " 934 | f"mode: '{body.name}'" 935 | ) 936 | kwargs["forces"] = body.get_attr_tensor("dof_force", 0) 937 | self._p.setJointMotorControlArray( 938 | self._body_ids[body.name], 939 | self._dof_indices[body.name], 940 | self._DOF_CONTROL_MODE_MAP[body.dof_control_mode.item()], 941 | **kwargs, 942 | ) 943 | if body.dof_control_mode.ndim == 1: 944 | if body.dof_max_force is not None: 945 | if ( 946 | DoFControlMode.POSITION_CONTROL not in body.dof_control_mode 947 | and DoFControlMode.VELOCITY_CONTROL not in body.dof_control_mode 948 | ): 949 | raise ValueError( 950 | "For Bullet, 'dof_max_force' can only be set in POSITION_CONTROL " 951 | f"and VELOCITY_CONTROL modes: '{body.name}'" 952 | ) 953 | kwargs["forces"] = body.get_attr_array("dof_max_force", 0) 954 | if DoFControlMode.POSITION_CONTROL in body.dof_control_mode: 955 | self._p.setJointMotorControlArray( 956 | self._body_ids[body.name], 957 | self._dof_indices[body.name][ 958 | body.dof_control_mode == DoFControlMode.POSITION_CONTROL 959 | ], 960 | self._DOF_CONTROL_MODE_MAP[DoFControlMode.POSITION_CONTROL], 961 | **{ 962 | k: v[body.dof_control_mode == DoFControlMode.POSITION_CONTROL] 963 | for k, v in kwargs.items() 964 | }, 965 | ) 966 | if DoFControlMode.VELOCITY_CONTROL in body.dof_control_mode: 967 | self._p.setJointMotorControlArray( 968 | self._body_ids[body.name], 969 | self._dof_indices[body.name][ 970 | body.dof_control_mode == DoFControlMode.VELOCITY_CONTROL 971 | ], 972 | self._DOF_CONTROL_MODE_MAP[DoFControlMode.VELOCITY_CONTROL], 973 | **{ 974 | k: v[body.dof_control_mode == DoFControlMode.VELOCITY_CONTROL] 975 | for k, v in kwargs.items() 976 | }, 977 | ) 978 | if "forces" in kwargs: 979 | del kwargs["forces"] 980 | if body.dof_force is not None: 981 | if DoFControlMode.TORQUE_CONTROL not in body.dof_control_mode: 982 | raise ValueError( 983 | "For Bullet, 'dof_force' can only be set in the TORQUE_CONTROL " 984 | f"mode: '{body.name}'" 985 | ) 986 | kwargs["forces"] = body.get_attr_tensor("dof_force", 0) 987 | if DoFControlMode.TORQUE_CONTROL in body.dof_control_mode: 988 | self._p.setJointMotorControlArray( 989 | self._body_ids[body.name], 990 | self._dof_indices[body.name][ 991 | body.dof_control_mode == DoFControlMode.TORQUE_CONTROL 992 | ], 993 | self._DOF_CONTROL_MODE_MAP[DoFControlMode.TORQUE_CONTROL], 994 | **{ 995 | k: v[body.dof_control_mode == DoFControlMode.TORQUE_CONTROL] 996 | for k, v in kwargs.items() 997 | }, 998 | ) 999 | else: 1000 | kwargs = {} 1001 | if body.dof_target_position is not None: 1002 | kwargs["targetPosition"] = body.get_attr_tensor("dof_target_position", 0) 1003 | if body.dof_target_velocity is not None: 1004 | kwargs["targetVelocity"] = body.get_attr_tensor("dof_target_velocity", 0) 1005 | if body.dof_max_velocity is not None: 1006 | # For Bullet, 'dof_max_velocity' has no effect when not in the POSITION_CONROL 1007 | # mode. 1008 | kwargs["maxVelocity"] = body.get_attr_array("dof_max_velocity", 0) 1009 | if body.dof_position_gain is not None: 1010 | kwargs["positionGain"] = body.get_attr_array("dof_position_gain", 0) 1011 | if body.dof_velocity_gain is not None: 1012 | kwargs["velocityGain"] = body.get_attr_array("dof_velocity_gain", 0) 1013 | if body.dof_control_mode.ndim == 0: 1014 | if body.dof_max_force is not None: 1015 | if body.dof_control_mode not in ( 1016 | DoFControlMode.POSITION_CONTROL, 1017 | DoFControlMode.VELOCITY_CONTROL, 1018 | ): 1019 | raise ValueError( 1020 | "For Bullet, 'dof_max_force' can only be set in POSITION_CONTROL " 1021 | f"and VELOCITY_CONTROL modes: '{body.name}'" 1022 | ) 1023 | kwargs["force"] = body.get_attr_array("dof_max_force", 0) 1024 | if body.dof_force is not None: 1025 | if body.dof_control_mode != DoFControlMode.TORQUE_CONTROL: 1026 | raise ValueError( 1027 | "For Bullet, 'dof_force' can only be set in the TORQUE_CONTROL " 1028 | f"mode: '{body.name}'" 1029 | ) 1030 | kwargs["force"] = body.get_attr_tensor("dof_force", 0) 1031 | for i, j in enumerate(self._dof_indices[body.name]): 1032 | self._p.setJointMotorControl2( 1033 | self._body_ids[body.name], 1034 | j, 1035 | self._DOF_CONTROL_MODE_MAP[body.dof_control_mode.item()], 1036 | **{k: v[i] for k, v in kwargs.items()}, 1037 | ) 1038 | if body.dof_control_mode.ndim == 1: 1039 | if ( 1040 | body.dof_max_force is not None 1041 | and DoFControlMode.POSITION_CONTROL not in body.dof_control_mode 1042 | and DoFControlMode.VELOCITY_CONTROL not in body.dof_control_mode 1043 | ): 1044 | raise ValueError( 1045 | "For Bullet, 'dof_max_force' can only be set in POSITION_CONTROL and " 1046 | f"VELOCITY_CONTROL modes: '{body.name}'" 1047 | ) 1048 | if ( 1049 | body.dof_force is not None 1050 | and DoFControlMode.TORQUE_CONTROL not in body.dof_control_mode 1051 | ): 1052 | raise ValueError( 1053 | "For Bullet, 'dof_force' can only be set in the TORQUE_CONTROL mode: " 1054 | f"'{body.name}'" 1055 | ) 1056 | for i, j in enumerate(self._dof_indices[body.name]): 1057 | if body.dof_control_mode[i] in ( 1058 | DoFControlMode.POSITION_CONTROL, 1059 | DoFControlMode.VELOCITY_CONTROL, 1060 | ): 1061 | if "force" in kwargs: 1062 | del kwargs["force"] 1063 | if body.dof_max_force is not None: 1064 | kwargs["force"] = body.get_attr_array("dof_max_force", 0) 1065 | if body.dof_force is not None and not np.isnan( 1066 | body.get_attr_tensor("dof_force", 0)[i] 1067 | ): 1068 | raise ValueError( 1069 | "For Bullet, 'dof_force' is required to be np.nan for DoF " 1070 | f"({i}) in POSITION_CONTROL and VELOCITY modes: {body.name}" 1071 | ) 1072 | self._p.setJointMotorControl2( 1073 | self._body_ids[body.name], 1074 | j, 1075 | self._DOF_CONTROL_MODE_MAP[body.dof_control_mode[i].item()], 1076 | **{k: v[i] for k, v in kwargs.items()}, 1077 | ) 1078 | if body.dof_control_mode[i] == DoFControlMode.TORQUE_CONTROL: 1079 | if "force" in kwargs: 1080 | del kwargs["force"] 1081 | if body.dof_force is not None: 1082 | kwargs["force"] = body.get_attr_tensor("dof_force", 0) 1083 | if body.dof_max_force is not None and not np.isnan( 1084 | body.get_attr_array("dof_max_force", 0)[i] 1085 | ): 1086 | raise ValueError( 1087 | "For Bullet, 'dof_max_force' is required to be np.nan for DoF " 1088 | f"({i}) in the TORQUE_CONTROL mode: {body.name}" 1089 | ) 1090 | self._p.setJointMotorControl2( 1091 | self._body_ids[body.name], 1092 | j, 1093 | self._DOF_CONTROL_MODE_MAP[body.dof_control_mode[i].item()], 1094 | **{k: v[i] for k, v in kwargs.items()}, 1095 | ) 1096 | 1097 | self._p.stepSimulation() 1098 | 1099 | self._clear_state() 1100 | self._clear_image() 1101 | self._contact = None 1102 | 1103 | @property 1104 | def contact(self): 1105 | """ """ 1106 | if self._contact is None: 1107 | self._contact = self._collect_contact() 1108 | return self._contact 1109 | 1110 | def _collect_contact(self): 1111 | """ """ 1112 | pts = self._p.getContactPoints() 1113 | if len(pts) == 0: 1114 | contact_array = create_contact_array(0) 1115 | else: 1116 | kwargs = {} 1117 | kwargs["body_id_a"] = [x[1] if x[1] != self._body_id_ground_plane else -1 for x in pts] 1118 | kwargs["body_id_b"] = [x[2] if x[2] != self._body_id_ground_plane else -1 for x in pts] 1119 | kwargs["link_id_a"] = [x[3] + 1 for x in pts] 1120 | kwargs["link_id_b"] = [x[4] + 1 for x in pts] 1121 | kwargs["position_a_world"] = [x[5] for x in pts] 1122 | kwargs["position_b_world"] = [x[6] for x in pts] 1123 | kwargs["position_a_link"] = np.nan 1124 | kwargs["position_b_link"] = np.nan 1125 | kwargs["normal"] = [x[7] for x in pts] 1126 | kwargs["force"] = [x[9] for x in pts] 1127 | contact_array = create_contact_array(len(pts), **kwargs) 1128 | return [contact_array] 1129 | 1130 | def close(self): 1131 | """ """ 1132 | if self._connected: 1133 | for name in self._plugins: 1134 | self._p.unloadPlugin(self._plugins[name]) 1135 | self._p.disconnect() 1136 | self._connected = False 1137 | -------------------------------------------------------------------------------- /src/easysim/simulators/isaac_gym.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import sys 6 | import os 7 | import numpy as np 8 | import torch 9 | import time 10 | 11 | try: 12 | from isaacgym import gymapi, gymtorch, gymutil 13 | except ImportError: 14 | # A temporary workaround for requirement of importing `isaacgym` before `torch` in 15 | # `isaacgym/gymdeps.py`. Remove this exception handler if this requirement has been removed. 16 | torch_module = sys.modules.pop("torch") 17 | from isaacgym import gymapi 18 | 19 | sys.modules["torch"] = torch_module 20 | from isaacgym import gymtorch, gymutil 21 | 22 | from easysim.simulators.simulator import Simulator 23 | from easysim.constants import GeometryType, DoFControlMode, MeshNormalMode 24 | from easysim.contact import create_contact_array 25 | 26 | 27 | class IsaacGym(Simulator): 28 | """Isaac Gym simulator.""" 29 | 30 | _ATTR_RIGID_SHAPE_PROPS = ( 31 | "link_collision_filter", 32 | "link_lateral_friction", 33 | "link_spinning_friction", 34 | "link_rolling_friction", 35 | "link_restitution", 36 | ) 37 | _ATTR_DOF_PROPS = ( 38 | "dof_has_limits", 39 | "dof_lower_limit", 40 | "dof_upper_limit", 41 | "dof_control_mode", 42 | "dof_max_force", 43 | "dof_max_velocity", 44 | "dof_position_gain", 45 | "dof_velocity_gain", 46 | "dof_armature", 47 | ) 48 | _ATTR_PROJECTION_MATRIX = ("width", "height", "vertical_fov", "near", "far") 49 | _ATTR_VIEW_MATRIX = ("position", "target", "up_vector", "orientation") 50 | _MESH_NORMAL_MODE_MAP = { 51 | MeshNormalMode.FROM_ASSET: gymapi.FROM_ASSET, 52 | MeshNormalMode.COMPUTE_PER_VERTEX: gymapi.COMPUTE_PER_VERTEX, 53 | MeshNormalMode.COMPUTE_PER_FACE: gymapi.COMPUTE_PER_FACE, 54 | } 55 | _DOF_CONTROL_MODE_MAP = { 56 | DoFControlMode.NONE: gymapi.DOF_MODE_NONE, 57 | DoFControlMode.POSITION_CONTROL: gymapi.DOF_MODE_POS, 58 | DoFControlMode.VELOCITY_CONTROL: gymapi.DOF_MODE_VEL, 59 | DoFControlMode.TORQUE_CONTROL: gymapi.DOF_MODE_EFFORT, 60 | } 61 | 62 | def __init__(self, cfg, scene): 63 | """ """ 64 | super().__init__(cfg, scene) 65 | 66 | x = self._cfg.SIM_DEVICE.split(":") 67 | sim_device_type = x[0] 68 | if len(x) > 1: 69 | self._sim_device_id = int(x[1]) 70 | else: 71 | self._sim_device_id = 0 72 | 73 | self._device = "cpu" 74 | if self._cfg.ISAAC_GYM.USE_GPU_PIPELINE: 75 | if sim_device_type == "cuda": 76 | self._device = "cuda:" + str(self._sim_device_id) 77 | else: 78 | print("GPU pipeline can only be used with GPU simulation. Forcing CPU pipeline.") 79 | self._cfg.ISAAC_GYM.USE_GPU_PIPELINE = False 80 | 81 | if ( 82 | not self._cfg.RENDER 83 | and not self._cfg.ISAAC_GYM.ENABLE_CAMERA_SENSORS 84 | and self._cfg.ISAAC_GYM.GRAPHICS_DEVICE_ID != -1 85 | ): 86 | self._cfg.ISAAC_GYM.GRAPHICS_DEVICE_ID = -1 87 | 88 | # Support only PhysX for now. 89 | self._physics_engine = gymapi.SIM_PHYSX 90 | self._sim_params = self._parse_sim_params(self._cfg, sim_device_type) 91 | 92 | self._num_envs = self._cfg.NUM_ENVS 93 | 94 | self._gym = gymapi.acquire_gym() 95 | 96 | self._created = False 97 | self._last_render_time = 0.0 98 | self._counter_render = 0 99 | self._render_time_step = max( 100 | 1.0 / self._cfg.ISAAC_GYM.RENDER_FRAME_RATE, self._cfg.TIME_STEP 101 | ) 102 | self._render_steps = self._render_time_step / self._cfg.TIME_STEP 103 | 104 | def _parse_sim_params(self, cfg, sim_device_type): 105 | """ """ 106 | sim_params = gymapi.SimParams() 107 | 108 | if cfg.USE_DEFAULT_STEP_PARAMS: 109 | cfg.TIME_STEP = sim_params.dt 110 | cfg.SUBSTEPS = sim_params.substeps 111 | else: 112 | sim_params.dt = cfg.TIME_STEP 113 | sim_params.substeps = cfg.SUBSTEPS 114 | sim_params.gravity = gymapi.Vec3(*cfg.GRAVITY) 115 | sim_params.up_axis = gymapi.UP_AXIS_Z 116 | sim_params.use_gpu_pipeline = cfg.ISAAC_GYM.USE_GPU_PIPELINE 117 | 118 | sim_params.physx.use_gpu = sim_device_type == "cuda" 119 | sim_params.physx.max_depenetration_velocity = cfg.ISAAC_GYM.PHYSX.MAX_DEPENETRATION_VELOCITY 120 | sim_params.physx.contact_collection = gymapi.ContactCollection( 121 | cfg.ISAAC_GYM.PHYSX.CONTACT_COLLECTION 122 | ) 123 | 124 | return sim_params 125 | 126 | def reset(self, env_ids): 127 | """ """ 128 | if not self._created: 129 | self._sim = self._create_sim( 130 | self._sim_device_id, 131 | self._cfg.ISAAC_GYM.GRAPHICS_DEVICE_ID, 132 | self._physics_engine, 133 | self._sim_params, 134 | ) 135 | 136 | if self._cfg.GROUND_PLANE.LOAD: 137 | self._load_ground_plane() 138 | self._load_assets() 139 | self._create_envs( 140 | self._num_envs, self._cfg.ISAAC_GYM.SPACING, int(np.sqrt(self._num_envs)) 141 | ) 142 | 143 | self._scene_cache = type(self._scene)() 144 | self._cache_body_and_set_props() 145 | self._set_callback_body() 146 | self._set_camera_props() 147 | self._set_callback_camera() 148 | self._set_callback_scene() 149 | 150 | self._gym.prepare_sim(self._sim) 151 | self._acquire_physics_state_tensors() 152 | 153 | self._set_viewer() 154 | self._allocate_buffers() 155 | 156 | self._created = True 157 | 158 | if env_ids is None: 159 | env_ids = torch.arange(self._num_envs, device=self._device) 160 | 161 | self._reset_idx(env_ids) 162 | 163 | self._graphics_stepped = False 164 | self._clear_state() 165 | self._clear_image() 166 | self._contact = None 167 | 168 | def _create_sim(self, compute_device, graphics_device, physics_engine, sim_params): 169 | """ """ 170 | sim = self._gym.create_sim( 171 | compute_device=compute_device, 172 | graphics_device=graphics_device, 173 | type=physics_engine, 174 | params=sim_params, 175 | ) 176 | if sim is None: 177 | raise RuntimeError("Failed to create sim") 178 | 179 | return sim 180 | 181 | def _load_ground_plane(self): 182 | """ """ 183 | plane_params = gymapi.PlaneParams() 184 | plane_params.normal = gymapi.Vec3(x=0.0, y=0.0, z=1.0) 185 | plane_params.distance = self._cfg.GROUND_PLANE.DISTANCE 186 | self._gym.add_ground(self._sim, plane_params) 187 | 188 | def _load_assets(self): 189 | """ """ 190 | self._assets = {} 191 | self._asset_num_dofs = {} 192 | self._asset_num_rigid_bodies = {} 193 | self._asset_num_rigid_shapes = {} 194 | self._asset_rigid_body_mapping = {-1: [0, 0]} 195 | 196 | counter_rigid_body = 0 197 | 198 | for b, body in enumerate(self._scene.bodies): 199 | asset_options = gymapi.AssetOptions() 200 | if body.use_fixed_base is not None: 201 | asset_options.fix_base_link = body.use_fixed_base 202 | if body.use_self_collision is not None: 203 | raise ValueError( 204 | "For Isaac Gym, keep 'use_self_collision' to None and set self-collision with " 205 | f"'collision_filter' (0: enabled): '{body.name}'" 206 | ) 207 | if body.link_linear_damping is not None: 208 | if body.link_linear_damping.ndim != 0: 209 | raise ValueError( 210 | "For Isaac Gym, 'link_linear_damping' must have a number of dimensions of " 211 | f"0: '{body.name}'" 212 | ) 213 | asset_options.linear_damping = body.link_linear_damping 214 | if body.link_angular_damping is not None: 215 | if body.link_angular_damping.ndim != 0: 216 | raise ValueError( 217 | "For Isaac Gym, 'link_angular_damping' must have a number of dimensions of " 218 | f"0: '{body.name}'" 219 | ) 220 | asset_options.angular_damping = body.link_angular_damping 221 | asset_options.override_com = True 222 | asset_options.override_inertia = True 223 | if body.vhacd_enabled is not None: 224 | asset_options.vhacd_enabled = body.vhacd_enabled 225 | if body.vhacd_params is not None: 226 | for attr in body.vhacd_params: 227 | setattr(asset_options.vhacd_params, attr, body.vhacd_params[attr]) 228 | asset_options.use_mesh_materials = True 229 | if body.mesh_normal_mode is not None: 230 | asset_options.mesh_normal_mode = self._MESH_NORMAL_MODE_MAP[body.mesh_normal_mode] 231 | 232 | if body.geometry_type is None: 233 | raise ValueError(f"For Isaac Gym, 'geometry_type' must not be None: '{body.name}'") 234 | if body.geometry_type not in (GeometryType.URDF, GeometryType.SPHERE, GeometryType.BOX): 235 | raise ValueError( 236 | f"For Isaac Gym, 'geometry_type' only supports URDF and SPHERE: '{body.name}'" 237 | ) 238 | if body.geometry_type == GeometryType.URDF: 239 | for attr in ("sphere_radius", "box_half_extent"): 240 | if getattr(body, attr) is not None: 241 | raise ValueError( 242 | f"'{attr}' must be None for geometry type URDF: '{body.name}'" 243 | ) 244 | asset_root, asset_file = os.path.split(body.urdf_file) 245 | self._assets[body.name] = self._gym.load_asset( 246 | self._sim, asset_root, asset_file, options=asset_options 247 | ) 248 | if body.geometry_type == GeometryType.SPHERE: 249 | for attr in ("urdf_file", "box_half_extent"): 250 | if getattr(body, attr) is not None: 251 | raise ValueError( 252 | f"'{attr}' must be None for geometry type SPHERE: '{body.name}'" 253 | ) 254 | if body.sphere_radius is None: 255 | raise ValueError( 256 | "For Isaac Gym, 'sphere_radius' must not be None if 'geometry_type' is set " 257 | f"to SPHERE: '{body.name}'" 258 | ) 259 | self._assets[body.name] = self._gym.create_sphere( 260 | self._sim, body.sphere_radius, options=asset_options 261 | ) 262 | if body.geometry_type == GeometryType.BOX: 263 | for attr in ("urdf_file", "sphere_radius"): 264 | if getattr(body, attr) is not None: 265 | raise ValueError( 266 | f"'{attr}' must be None for geometry type BOX: '{body.name}'" 267 | ) 268 | if body.box_half_extent is None: 269 | raise ValueError( 270 | "For Isaac Gym, 'box_half_extent' must not be None if 'geometry_type' is " 271 | f"set to BOX: '{body.name}'" 272 | ) 273 | self._assets[body.name] = self._gym.create_box( 274 | self._sim, *[x * 2 for x in body.box_half_extent], options=asset_options 275 | ) 276 | 277 | self._asset_num_dofs[body.name] = self._gym.get_asset_dof_count(self._assets[body.name]) 278 | self._asset_num_rigid_bodies[body.name] = self._gym.get_asset_rigid_body_count( 279 | self._assets[body.name] 280 | ) 281 | self._asset_num_rigid_shapes[body.name] = self._gym.get_asset_rigid_shape_count( 282 | self._assets[body.name] 283 | ) 284 | 285 | for i in range(self._asset_num_rigid_bodies[body.name]): 286 | self._asset_rigid_body_mapping[counter_rigid_body + i] = [b, i] 287 | counter_rigid_body += self._asset_num_rigid_bodies[body.name] 288 | 289 | def _create_envs(self, num_envs, spacing, num_per_row): 290 | """ """ 291 | lower = gymapi.Vec3(x=-spacing, y=-spacing, z=0.0) 292 | upper = gymapi.Vec3(x=+spacing, y=+spacing, z=spacing) 293 | 294 | self._envs = [] 295 | 296 | self._actor_handles = [{} for _ in range(num_envs)] 297 | self._actor_indices = [[] for _ in range(num_envs)] 298 | self._actor_indices_need_filter = False 299 | 300 | self._actor_root_indices = {body.name: [] for body in self._scene.bodies} 301 | self._dof_indices = {body.name: [] for body in self._scene.bodies} 302 | self._rigid_body_indices = {body.name: [] for body in self._scene.bodies} 303 | counter_actor = 0 304 | counter_dof = 0 305 | counter_rigid_body = 0 306 | 307 | contact_id = {body.name: [] for body in self._scene.bodies} 308 | 309 | if len(self._scene.cameras) > 0 and not self._cfg.ISAAC_GYM.ENABLE_CAMERA_SENSORS: 310 | raise ValueError( 311 | "For Isaac Gym, ISAAC_GYM.ENABLE_CAMERA_SENSORS must be True if any cameras is used" 312 | ) 313 | 314 | self._camera_handles = [{} for _ in range(num_envs)] 315 | self._camera_image_buffer = [{} for _ in range(num_envs)] 316 | 317 | for i in range(num_envs): 318 | env_ptr = self._gym.create_env(self._sim, lower, upper, num_per_row) 319 | 320 | counter_body = 0 321 | 322 | for body in self._scene.bodies: 323 | if body.env_ids_load is not None and i not in body.env_ids_load: 324 | self._actor_indices[i].append(-1) 325 | if not self._actor_indices_need_filter: 326 | self._actor_indices_need_filter = True 327 | contact_id[body.name].append(-2) 328 | continue 329 | 330 | actor_handle = self._gym.create_actor( 331 | env_ptr, self._assets[body.name], gymapi.Transform(), name=body.name, group=i 332 | ) 333 | actor_index = self._gym.get_actor_index(env_ptr, actor_handle, gymapi.DOMAIN_SIM) 334 | self._actor_handles[i][body.name] = actor_handle 335 | self._actor_indices[i].append(actor_index) 336 | 337 | self._actor_root_indices[body.name].append(counter_actor) 338 | self._dof_indices[body.name].append(counter_dof) 339 | self._rigid_body_indices[body.name].append(counter_rigid_body) 340 | counter_actor += 1 341 | counter_dof += self._asset_num_dofs[body.name] 342 | counter_rigid_body += self._asset_num_rigid_bodies[body.name] 343 | 344 | contact_id[body.name].append(counter_body) 345 | counter_body += 1 346 | 347 | for camera in self._scene.cameras: 348 | camera_props = self._make_camera_props(camera, i) 349 | camera_handle = self._gym.create_camera_sensor(env_ptr, camera_props) 350 | 351 | color = self._gym.get_camera_image_gpu_tensor( 352 | self._sim, env_ptr, camera_handle, gymapi.IMAGE_COLOR 353 | ) 354 | depth = self._gym.get_camera_image_gpu_tensor( 355 | self._sim, env_ptr, camera_handle, gymapi.IMAGE_DEPTH 356 | ) 357 | segmentation = self._gym.get_camera_image_gpu_tensor( 358 | self._sim, env_ptr, camera_handle, gymapi.IMAGE_SEGMENTATION 359 | ) 360 | 361 | self._camera_handles[i][camera.name] = camera_handle 362 | self._camera_image_buffer[i][camera.name] = {} 363 | self._camera_image_buffer[i][camera.name]["color"] = gymtorch.wrap_tensor(color) 364 | self._camera_image_buffer[i][camera.name]["depth"] = gymtorch.wrap_tensor(depth) 365 | self._camera_image_buffer[i][camera.name]["segmentation"] = gymtorch.wrap_tensor( 366 | segmentation 367 | ) 368 | 369 | self._envs.append(env_ptr) 370 | 371 | self._actor_indices = torch.tensor( 372 | self._actor_indices, dtype=torch.int32, device=self._device 373 | ) 374 | for body in self._scene.bodies: 375 | self._actor_root_indices[body.name] = torch.tensor( 376 | self._actor_root_indices[body.name], dtype=torch.int64, device=self._device 377 | ) 378 | self._dof_indices[body.name] = torch.tensor( 379 | self._dof_indices[body.name], dtype=torch.int64, device=self._device 380 | ) 381 | self._rigid_body_indices[body.name] = torch.tensor( 382 | self._rigid_body_indices[body.name], dtype=torch.int64, device=self._device 383 | ) 384 | body.contact_id = contact_id[body.name] 385 | 386 | def _make_camera_props(self, camera, idx): 387 | """ """ 388 | camera_props = gymapi.CameraProperties() 389 | 390 | if camera.width is not None: 391 | camera_props.width = camera.get_attr_array("width", idx) 392 | if camera.height is not None: 393 | camera_props.height = camera.get_attr_array("height", idx) 394 | if camera.vertical_fov is not None: 395 | if camera.width is None or camera.height is None: 396 | raise ValueError( 397 | "For Isaac Gym, cannot set 'vertical_fov' without setting 'width' and " 398 | f"'height': '{camera.name}'" 399 | ) 400 | camera_props.horizontal_fov = ( 401 | camera.get_attr_array("vertical_fov", idx) 402 | * camera.get_attr_array("width", idx) 403 | / camera.get_attr_array("height", idx) 404 | ) 405 | if camera.near is not None: 406 | camera_props.near_plane = camera.get_attr_array("near", idx) 407 | if camera.far is not None: 408 | camera_props.far_plane = camera.get_attr_array("far", idx) 409 | camera_props.enable_tensors = True 410 | 411 | return camera_props 412 | 413 | def _cache_body_and_set_props(self): 414 | """ """ 415 | for body in self._scene.bodies: 416 | x = type(body)() 417 | x.name = body.name 418 | self._scene_cache.add_body(x) 419 | 420 | if self._asset_num_dofs[body.name] == 0: 421 | for attr in self._ATTR_DOF_PROPS: 422 | if getattr(body, attr) is not None: 423 | raise ValueError( 424 | f"'{attr}' must be None for body with 0 DoF: '{body.name}'" 425 | ) 426 | 427 | for idx in range(self._num_envs): 428 | if body.env_ids_load is not None and idx not in body.env_ids_load: 429 | continue 430 | 431 | if body.scale is not None: 432 | self._set_scale(body, idx) 433 | 434 | if any(getattr(body, x) is not None for x in self._ATTR_RIGID_SHAPE_PROPS): 435 | self._set_rigid_shape_props(body, idx) 436 | 437 | if body.link_color is not None: 438 | self._set_link_color(body, idx) 439 | 440 | if body.link_segmentation_id is not None: 441 | self._set_link_segmentation_id(body, idx) 442 | 443 | if self._asset_num_dofs[body.name] > 0 and any( 444 | getattr(body, x) is not None for x in self._ATTR_DOF_PROPS 445 | ): 446 | self._set_dof_props(body, idx) 447 | 448 | if body.scale is None: 449 | scale = [] 450 | for idx in range(self._num_envs): 451 | if body.env_ids_load is not None and idx not in body.env_ids_load: 452 | scale_idx = 1.0 453 | else: 454 | scale_idx = self._gym.get_actor_scale( 455 | self._envs[idx], self._actor_handles[idx][body.name] 456 | ) 457 | scale.append(scale_idx) 458 | body.scale = scale 459 | 460 | if any(getattr(body, x) is None for x in self._ATTR_RIGID_SHAPE_PROPS): 461 | rigid_shape_props = self._gym.get_asset_rigid_shape_properties( 462 | self._assets[body.name] 463 | ) 464 | if body.link_collision_filter is None: 465 | body.link_collision_filter = [ 466 | [prop.filter for prop in rigid_shape_props] 467 | ] * self._num_envs 468 | if body.link_lateral_friction is None: 469 | body.link_lateral_friction = [ 470 | [prop.friction for prop in rigid_shape_props] 471 | ] * self._num_envs 472 | if body.link_spinning_friction is None: 473 | body.link_spinning_friction = [ 474 | [prop.torsion_friction for prop in rigid_shape_props] 475 | ] * self._num_envs 476 | if body.link_rolling_friction is None: 477 | body.link_rolling_friction = [ 478 | [prop.rolling_friction for prop in rigid_shape_props] 479 | ] * self._num_envs 480 | if body.link_restitution is None: 481 | body.link_restitution = [ 482 | [prop.restitution for prop in rigid_shape_props] 483 | ] * self._num_envs 484 | 485 | if body.link_color is None: 486 | # Avoid error from `get_rigid_body_color()` when `graphics_device` is set to -1. 487 | if self._cfg.ISAAC_GYM.GRAPHICS_DEVICE_ID == -1: 488 | body.link_color = [ 489 | [[1.0, 1.0, 1.0]] * self._asset_num_rigid_bodies[body.name] 490 | ] * self._num_envs 491 | else: 492 | link_color = [] 493 | for idx in range(self._num_envs): 494 | if body.env_ids_load is not None and idx not in body.env_ids_load: 495 | link_color_idx = [[1.0, 1.0, 1.0]] * self._asset_num_rigid_bodies[ 496 | body.name 497 | ] 498 | else: 499 | link_color_idx = [] 500 | for i in range(self._asset_num_rigid_bodies[body.name]): 501 | rigid_body_color = self._gym.get_rigid_body_color( 502 | self._envs[idx], 503 | self._actor_handles[idx][body.name], 504 | i, 505 | gymapi.MESH_VISUAL, 506 | ) 507 | link_color_idx.append( 508 | [rigid_body_color.x, rigid_body_color.y, rigid_body_color.z] 509 | ) 510 | link_color.append(link_color_idx) 511 | body.link_color = link_color 512 | 513 | if self._asset_num_dofs[body.name] > 0 and any( 514 | getattr(body, x) is None for x in self._ATTR_DOF_PROPS 515 | ): 516 | dof_props = self._gym.get_asset_dof_properties(self._assets[body.name]) 517 | if body.dof_has_limits is None: 518 | body.dof_has_limits = np.tile(dof_props["hasLimits"], (self._num_envs, 1)) 519 | if body.dof_lower_limit is None: 520 | body.dof_lower_limit = np.tile(dof_props["lower"], (self._num_envs, 1)) 521 | if body.dof_upper_limit is None: 522 | body.dof_upper_limit = np.tile(dof_props["upper"], (self._num_envs, 1)) 523 | if body.dof_control_mode is None: 524 | body.dof_control_mode = [ 525 | k 526 | for x in dof_props["driveMode"] 527 | for k, v in self._DOF_CONTROL_MODE_MAP.items() 528 | if x == v 529 | ] 530 | if body.dof_max_velocity is None: 531 | body.dof_max_velocity = np.tile(dof_props["velocity"], (self._num_envs, 1)) 532 | if body.dof_max_force is None: 533 | body.dof_max_force = np.tile(dof_props["effort"], (self._num_envs, 1)) 534 | if body.dof_position_gain is None: 535 | body.dof_position_gain = np.tile(dof_props["stiffness"], (self._num_envs, 1)) 536 | if body.dof_velocity_gain is None: 537 | body.dof_velocity_gain = np.tile(dof_props["damping"], (self._num_envs, 1)) 538 | if body.dof_armature is None: 539 | body.dof_armature = np.tile(dof_props["armature"], (self._num_envs, 1)) 540 | 541 | body.lock_attr_array() 542 | 543 | def _set_scale(self, body, idx): 544 | """ """ 545 | self._gym.set_actor_scale( 546 | self._envs[idx], self._actor_handles[idx][body.name], body.get_attr_array("scale", idx) 547 | ) 548 | 549 | def _set_rigid_shape_props(self, body, idx): 550 | """ """ 551 | for attr in self._ATTR_RIGID_SHAPE_PROPS: 552 | if ( 553 | not body.attr_array_locked[attr] 554 | and getattr(body, attr) is not None 555 | and len(body.get_attr_array(attr, idx)) != self._asset_num_rigid_shapes[body.name] 556 | ): 557 | raise ValueError( 558 | f"Size of '{attr}' in the link dimension " 559 | f"({len(body.get_attr_array(attr, idx))}) should match the number of rigid " 560 | f"shapes ({self._asset_num_rigid_shapes[body.name]}): '{body.name}'" 561 | ) 562 | rigid_shape_props = self._gym.get_actor_rigid_shape_properties( 563 | self._envs[idx], self._actor_handles[idx][body.name] 564 | ) 565 | if ( 566 | not body.attr_array_locked["link_collision_filter"] 567 | and body.link_collision_filter is not None 568 | or body.attr_array_dirty_flag["link_collision_filter"] 569 | ): 570 | link_collision_filter = body.get_attr_array("link_collision_filter", idx) 571 | for i, prop in enumerate(rigid_shape_props): 572 | prop.filter = link_collision_filter[i] 573 | if ( 574 | not body.attr_array_locked["link_lateral_friction"] 575 | and body.link_lateral_friction is not None 576 | or body.attr_array_dirty_flag["link_lateral_friction"] 577 | ): 578 | link_lateral_friction = body.get_attr_array("link_lateral_friction", idx) 579 | for i, prop in enumerate(rigid_shape_props): 580 | prop.friction = link_lateral_friction[i] 581 | if ( 582 | not body.attr_array_locked["link_spinning_friction"] 583 | and body.link_spinning_friction is not None 584 | or body.attr_array_dirty_flag["link_spinning_friction"] 585 | ): 586 | link_spinning_friction = body.get_attr_array("link_spinning_friction", idx) 587 | for i, prop in enumerate(rigid_shape_props): 588 | prop.torsion_friction = link_spinning_friction[i] 589 | if ( 590 | not body.attr_array_locked["link_rolling_friction"] 591 | and body.link_rolling_friction is not None 592 | or body.attr_array_dirty_flag["link_rolling_friction"] 593 | ): 594 | link_rolling_friction = body.get_attr_array("link_rolling_friction", idx) 595 | for i, prop in enumerate(rigid_shape_props): 596 | prop.rolling_friction = link_rolling_friction[i] 597 | if ( 598 | not body.attr_array_locked["link_restitution"] 599 | and body.link_restitution is not None 600 | or body.attr_array_dirty_flag["link_restitution"] 601 | ): 602 | link_restitution = body.get_attr_array("link_restitution", idx) 603 | for i, prop in enumerate(rigid_shape_props): 604 | prop.restitution = link_restitution[i] 605 | self._gym.set_actor_rigid_shape_properties( 606 | self._envs[idx], self._actor_handles[idx][body.name], rigid_shape_props 607 | ) 608 | 609 | def _set_link_color(self, body, idx): 610 | """ """ 611 | link_color = body.get_attr_array("link_color", idx) 612 | if ( 613 | not body.attr_array_locked["link_color"] 614 | and len(link_color) != self._asset_num_rigid_bodies[body.name] 615 | ): 616 | raise ValueError( 617 | f"Size of 'link_color' in the link dimension ({len(link_color)}) should match the " 618 | f"number of links ({self._asset_num_rigid_bodies[body.name]}): '{body.name}'" 619 | ) 620 | for i in range(self._asset_num_rigid_bodies[body.name]): 621 | self._gym.set_rigid_body_color( 622 | self._envs[idx], 623 | self._actor_handles[idx][body.name], 624 | i, 625 | gymapi.MESH_VISUAL, 626 | gymapi.Vec3(*link_color[i]), 627 | ) 628 | 629 | def _set_link_segmentation_id(self, body, idx): 630 | """ """ 631 | link_segmentation_id = body.get_attr_array("link_segmentation_id", idx) 632 | if ( 633 | not body.attr_array_locked["link_segmentation_id"] 634 | and len(link_segmentation_id) != self._asset_num_rigid_bodies[body.name] 635 | ): 636 | raise ValueError( 637 | "Size of 'link_segmentation_id' in the link dimension " 638 | f"({len(link_segmentation_id)}) should match the number of links " 639 | f"({self._asset_num_rigid_bodies[body.name]}): '{body.name}'" 640 | ) 641 | for i in range(self._asset_num_rigid_bodies[body.name]): 642 | self._gym.set_rigid_body_segmentation_id( 643 | self._envs[idx], self._actor_handles[idx][body.name], i, link_segmentation_id[i] 644 | ) 645 | 646 | def _set_dof_props(self, body, idx, set_drive_mode=True): 647 | """ """ 648 | dof_props = self._gym.get_actor_dof_properties( 649 | self._envs[idx], self._actor_handles[idx][body.name] 650 | ) 651 | if ( 652 | not body.attr_array_locked["dof_has_limits"] 653 | and body.dof_has_limits is not None 654 | or body.attr_array_dirty_flag["dof_has_limits"] 655 | ): 656 | dof_props["hasLimits"] = body.get_attr_array("dof_has_limits", idx) 657 | if ( 658 | not body.attr_array_locked["dof_lower_limit"] 659 | and body.dof_lower_limit is not None 660 | or body.attr_array_dirty_flag["dof_lower_limit"] 661 | ): 662 | dof_props["lower"] = body.get_attr_array("dof_lower_limit", idx) 663 | if ( 664 | not body.attr_array_locked["dof_upper_limit"] 665 | and body.dof_upper_limit is not None 666 | or body.attr_array_dirty_flag["dof_upper_limit"] 667 | ): 668 | dof_props["upper"] = body.get_attr_array("dof_upper_limit", idx) 669 | if set_drive_mode: 670 | if body.dof_control_mode is not None: 671 | if body.dof_control_mode.ndim == 0: 672 | dof_props["driveMode"] = self._DOF_CONTROL_MODE_MAP[ 673 | body.dof_control_mode.item() 674 | ] 675 | if body.dof_control_mode.ndim == 1: 676 | dof_props["driveMode"] = [ 677 | self._DOF_CONTROL_MODE_MAP[x] for x in body.dof_control_mode 678 | ] 679 | if ( 680 | not body.attr_array_locked["dof_max_velocity"] 681 | and body.dof_max_velocity is not None 682 | or body.attr_array_dirty_flag["dof_max_velocity"] 683 | ): 684 | dof_props["velocity"] = body.get_attr_array("dof_max_velocity", idx) 685 | if ( 686 | not body.attr_array_locked["dof_max_force"] 687 | and body.dof_max_force is not None 688 | or body.attr_array_dirty_flag["dof_max_force"] 689 | ): 690 | dof_props["effort"] = body.get_attr_array("dof_max_force", idx) 691 | if ( 692 | not body.attr_array_locked["dof_position_gain"] 693 | and body.dof_position_gain is not None 694 | or body.attr_array_dirty_flag["dof_position_gain"] 695 | ): 696 | dof_props["stiffness"] = body.get_attr_array("dof_position_gain", idx) 697 | if ( 698 | not body.attr_array_locked["dof_velocity_gain"] 699 | and body.dof_velocity_gain is not None 700 | or body.attr_array_dirty_flag["dof_velocity_gain"] 701 | ): 702 | dof_props["damping"] = body.get_attr_array("dof_velocity_gain", idx) 703 | if ( 704 | not body.attr_array_locked["dof_armature"] 705 | and body.dof_armature is not None 706 | or body.attr_array_dirty_flag["dof_armature"] 707 | ): 708 | dof_props["armature"] = body.get_attr_array("dof_armature", idx) 709 | self._gym.set_actor_dof_properties( 710 | self._envs[idx], self._actor_handles[idx][body.name], dof_props 711 | ) 712 | 713 | def _set_callback_body(self): 714 | """ """ 715 | for body in self._scene.bodies: 716 | body.set_callback_collect_dof_state(self._collect_dof_state) 717 | body.set_callback_collect_link_state(self._collect_link_state) 718 | 719 | def _collect_dof_state(self, body): 720 | """ """ 721 | if not self._dof_state_refreshed: 722 | self._gym.refresh_dof_state_tensor(self._sim) 723 | self._dof_state_refreshed = True 724 | 725 | if self._asset_num_dofs[body.name] > 0: 726 | if body.env_ids_load is None: 727 | body.dof_state = torch.as_strided( 728 | self._dof_state, 729 | ( 730 | len(self._dof_state) - self._asset_num_dofs[body.name] + 1, 731 | self._asset_num_dofs[body.name], 732 | 2, 733 | ), 734 | (2, 2, 1), 735 | )[self._dof_indices[body.name]] 736 | else: 737 | body.dof_state = self._dof_state.new_zeros( 738 | (self._num_envs, self._asset_num_dofs[body.name], 2) 739 | ) 740 | body.dof_state[body.env_ids_load] = torch.as_strided( 741 | self._dof_state, 742 | ( 743 | len(self._dof_state) - self._asset_num_dofs[body.name] + 1, 744 | self._asset_num_dofs[body.name], 745 | 2, 746 | ), 747 | (2, 2, 1), 748 | )[self._dof_indices[body.name]] 749 | 750 | def _collect_link_state(self, body): 751 | """ """ 752 | if not self._link_state_refreshed: 753 | self._gym.refresh_rigid_body_state_tensor(self._sim) 754 | self._link_state_refreshed = True 755 | 756 | if body.env_ids_load is None: 757 | body.link_state = torch.as_strided( 758 | self._rigid_body_state, 759 | ( 760 | len(self._rigid_body_state) - self._asset_num_rigid_bodies[body.name] + 1, 761 | self._asset_num_rigid_bodies[body.name], 762 | 13, 763 | ), 764 | (13, 13, 1), 765 | )[self._rigid_body_indices[body.name]] 766 | else: 767 | body.link_state = self._rigid_body_state.new_zeros( 768 | (self._num_envs, self._asset_num_rigid_bodies[body.name], 13) 769 | ) 770 | body.link_state[body.env_ids_load] = torch.as_strided( 771 | self._rigid_body_state, 772 | ( 773 | len(self._rigid_body_state) - self._asset_num_rigid_bodies[body.name] + 1, 774 | self._asset_num_rigid_bodies[body.name], 775 | 13, 776 | ), 777 | (13, 13, 1), 778 | )[self._rigid_body_indices[body.name]] 779 | 780 | def _set_camera_props(self): 781 | """ """ 782 | for camera in self._scene.cameras: 783 | for idx in range(self._num_envs): 784 | self._set_camera_pose(camera, idx) 785 | 786 | if any(getattr(camera, x) is None for x in self._ATTR_PROJECTION_MATRIX): 787 | camera_props = gymapi.CameraProperties() 788 | if camera.width is None: 789 | camera.width = [camera_props.width] * self._num_envs 790 | if camera.height is None: 791 | camera.height = [camera_props.height] * self._num_envs 792 | if camera.vertical_fov is None: 793 | camera.vertical_fov = ( 794 | [camera_props.horizontal_fov] 795 | * self._num_envs 796 | * camera.height 797 | / camera.width 798 | ) 799 | if camera.near is None: 800 | camera.near = [camera_props.near_plane] * self._num_envs 801 | if camera.far is None: 802 | camera.far = [camera_props.far_plane] * self._num_envs 803 | 804 | camera.lock_attr_array() 805 | 806 | def _set_camera_pose(self, camera, idx): 807 | """ """ 808 | for attr in ("up_vector",): 809 | if getattr(camera, attr) is not None: 810 | raise ValueError(f"'{attr}' is not supported in Isaac Gym: '{camera.name}'") 811 | if camera.target is not None: 812 | self._gym.set_camera_location( 813 | self._camera_handles[idx][camera.name], 814 | self._envs[idx], 815 | gymapi.Vec3(*camera.get_attr_array("position", idx)), 816 | gymapi.Vec3(*camera.get_attr_array("target", idx)), 817 | ) 818 | elif camera.orientation is not None: 819 | # A rotation offset is required for gymapi.UP_AXIS_Z. 820 | self._gym.set_camera_transform( 821 | self._camera_handles[idx][camera.name], 822 | self._envs[idx], 823 | gymapi.Transform( 824 | p=gymapi.Vec3(*camera.get_attr_array("position", idx)), 825 | r=gymapi.Quat(*camera.get_attr_array("orientation", idx)) 826 | * gymapi.Quat(x=-0.5, y=+0.5, z=+0.5, w=+0.5), 827 | ), 828 | ) 829 | else: 830 | raise ValueError( 831 | "For Isaac Gym, either 'target' or 'orientation' is required to be set: " 832 | f"{camera.name}" 833 | ) 834 | 835 | def _set_callback_camera(self): 836 | """ """ 837 | for camera in self._scene.cameras: 838 | camera.set_callback_render_color(self._render_color) 839 | camera.set_callback_render_depth(self._render_depth) 840 | camera.set_callback_render_segmentation(self._render_segmentation) 841 | 842 | def _render_color(self, camera): 843 | """ """ 844 | for body in self._scene.bodies: 845 | if body.attr_array_dirty_flag["link_color"]: 846 | env_ids_masked = np.nonzero(body.attr_array_dirty_mask["link_color"])[0] 847 | for idx in env_ids_masked: 848 | self._set_link_color(body, idx) 849 | body.attr_array_dirty_flag["link_color"] = False 850 | body.attr_array_dirty_mask["link_color"][:] = False 851 | if self._all_camera_rendered: 852 | self._all_camera_rendered = False 853 | 854 | self._check_and_step_graphics() 855 | self._check_and_update_camera_props(camera) 856 | self._check_and_render_all_camera() 857 | 858 | color = self._camera_image_buffer[0][camera.name]["color"].new_zeros( 859 | (self._num_envs, np.max(camera.height), np.max(camera.width), 4) 860 | ) 861 | 862 | self._gym.start_access_image_tensors(self._sim) 863 | for idx in range(self._num_envs): 864 | color[ 865 | idx, : camera.get_attr_array("height", idx), : camera.get_attr_array("width", idx) 866 | ] = self._camera_image_buffer[idx][camera.name]["color"] 867 | self._gym.end_access_image_tensors(self._sim) 868 | 869 | camera.color = color 870 | 871 | def _render_depth(self, camera): 872 | """ """ 873 | self._check_and_step_graphics() 874 | self._check_and_update_camera_props(camera) 875 | self._check_and_render_all_camera() 876 | 877 | depth = self._camera_image_buffer[0][camera.name]["depth"].new_zeros( 878 | (self._num_envs, np.max(camera.height), np.max(camera.width)) 879 | ) 880 | 881 | self._gym.start_access_image_tensors(self._sim) 882 | for idx in range(self._num_envs): 883 | depth[ 884 | idx, : camera.get_attr_array("height", idx), : camera.get_attr_array("width", idx) 885 | ] = self._camera_image_buffer[idx][camera.name]["depth"] 886 | self._gym.end_access_image_tensors(self._sim) 887 | 888 | depth *= -1.0 889 | depth[depth == +np.inf] = 0.0 890 | 891 | camera.depth = depth 892 | 893 | def _render_segmentation(self, camera): 894 | """ """ 895 | for body in self._scene.bodies: 896 | if body.attr_array_dirty_flag["link_segmentation_id"]: 897 | env_ids_masked = np.nonzero(body.attr_array_dirty_mask["link_segmentation_id"])[0] 898 | for idx in env_ids_masked: 899 | self._set_link_segmentation_id(body, idx) 900 | body.attr_array_dirty_flag["link_segmentation_id"] = False 901 | body.attr_array_dirty_mask["link_segmentation_id"][:] = False 902 | if self._all_camera_rendered: 903 | self._all_camera_rendered = False 904 | 905 | self._check_and_step_graphics() 906 | self._check_and_update_camera_props(camera) 907 | self._check_and_render_all_camera() 908 | 909 | segmentation = self._camera_image_buffer[0][camera.name]["segmentation"].new_zeros( 910 | (self._num_envs, np.max(camera.height), np.max(camera.width)) 911 | ) 912 | 913 | self._gym.start_access_image_tensors(self._sim) 914 | for idx in range(self._num_envs): 915 | segmentation[ 916 | idx, : camera.get_attr_array("height", idx), : camera.get_attr_array("width", idx) 917 | ] = self._camera_image_buffer[idx][camera.name]["segmentation"] 918 | self._gym.end_access_image_tensors(self._sim) 919 | 920 | camera.segmentation = segmentation 921 | 922 | def _check_and_step_graphics(self): 923 | """ """ 924 | if not self._graphics_stepped: 925 | self._gym.step_graphics(self._sim) 926 | self._graphics_stepped = True 927 | 928 | def _check_and_update_camera_props(self, camera): 929 | """ """ 930 | if any(camera.attr_array_dirty_flag[x] for x in self._ATTR_VIEW_MATRIX): 931 | mask = np.zeros(self._num_envs, dtype=bool) 932 | for attr in self._ATTR_VIEW_MATRIX: 933 | if camera.attr_array_dirty_flag[attr]: 934 | mask |= camera.attr_array_dirty_mask[attr] 935 | env_ids_masked = np.nonzero(mask)[0] 936 | for idx in env_ids_masked: 937 | self._set_camera_pose(camera, idx) 938 | for attr in self._ATTR_VIEW_MATRIX: 939 | if camera.attr_array_dirty_flag[attr]: 940 | camera.attr_array_dirty_flag[attr] = False 941 | camera.attr_array_dirty_mask[attr][:] = False 942 | if self._all_camera_rendered: 943 | self._all_camera_rendered = False 944 | 945 | def _check_and_render_all_camera(self): 946 | """ """ 947 | if not self._all_camera_rendered: 948 | self._gym.render_all_camera_sensors(self._sim) 949 | self._all_camera_rendered = True 950 | 951 | def _set_callback_scene(self): 952 | """ """ 953 | self._scene.set_callback_add_camera(self._add_camera) 954 | self._scene.set_callback_remove_camera(self._remove_camera) 955 | 956 | def _add_camera(self, camera): 957 | """ """ 958 | raise ValueError( 959 | "For Isaac Gym, the set of cameras cannot be altered after the first reset" 960 | ) 961 | 962 | def _remove_camera(self, camera): 963 | """ """ 964 | raise ValueError( 965 | "For Isaac Gym, the set of cameras cannot be altered after the first reset" 966 | ) 967 | 968 | def _acquire_physics_state_tensors(self): 969 | """ """ 970 | actor_root_state = self._gym.acquire_actor_root_state_tensor(self._sim) 971 | dof_state = self._gym.acquire_dof_state_tensor(self._sim) 972 | rigid_body_state = self._gym.acquire_rigid_body_state_tensor(self._sim) 973 | 974 | self._gym.refresh_actor_root_state_tensor(self._sim) 975 | self._gym.refresh_dof_state_tensor(self._sim) 976 | 977 | self._actor_root_state = gymtorch.wrap_tensor(actor_root_state) 978 | self._dof_state = gymtorch.wrap_tensor(dof_state) 979 | self._rigid_body_state = gymtorch.wrap_tensor(rigid_body_state) 980 | 981 | if self._actor_root_state is None: 982 | self._initial_actor_root_state = None 983 | else: 984 | self._initial_actor_root_state = self._actor_root_state.clone() 985 | if self._dof_state is None: 986 | self._initial_dof_state = None 987 | else: 988 | self._initial_dof_state = self._dof_state.clone() 989 | 990 | def _set_viewer(self): 991 | """ """ 992 | self._enable_viewer_sync = True 993 | self._viewer = None 994 | 995 | if self._cfg.RENDER: 996 | self._viewer = self._gym.create_viewer(self._sim, gymapi.CameraProperties()) 997 | self._gym.subscribe_viewer_keyboard_event(self._viewer, gymapi.KEY_ESCAPE, "quit") 998 | self._gym.subscribe_viewer_keyboard_event( 999 | self._viewer, gymapi.KEY_V, "toggle_viewer_sync" 1000 | ) 1001 | 1002 | if ( 1003 | self._cfg.INIT_VIEWER_CAMERA_POSITION 1004 | != ( 1005 | None, 1006 | None, 1007 | None, 1008 | ) 1009 | and self._cfg.INIT_VIEWER_CAMERA_TARGET != (None, None, None) 1010 | ): 1011 | self._gym.viewer_camera_look_at( 1012 | self._viewer, 1013 | None, 1014 | gymapi.Vec3(*self._cfg.INIT_VIEWER_CAMERA_POSITION), 1015 | gymapi.Vec3(*self._cfg.INIT_VIEWER_CAMERA_TARGET), 1016 | ) 1017 | 1018 | if self._cfg.DRAW_VIEWER_AXES: 1019 | axes_geom = gymutil.AxesGeometry(1.0) 1020 | for env_ptr in self._envs: 1021 | gymutil.draw_lines( 1022 | axes_geom, self._gym, self._viewer, env_ptr, gymapi.Transform() 1023 | ) 1024 | 1025 | def _allocate_buffers(self): 1026 | """ """ 1027 | if self._dof_state is None: 1028 | self._dof_position_target_buffer = None 1029 | self._dof_velocity_target_buffer = None 1030 | self._dof_actuation_force_buffer = None 1031 | else: 1032 | self._dof_position_target_buffer = torch.zeros( 1033 | len(self._dof_state), dtype=torch.float32, device=self._device 1034 | ) 1035 | self._dof_velocity_target_buffer = torch.zeros( 1036 | len(self._dof_state), dtype=torch.float32, device=self._device 1037 | ) 1038 | self._dof_actuation_force_buffer = torch.zeros( 1039 | len(self._dof_state), dtype=torch.float32, device=self._device 1040 | ) 1041 | 1042 | def _reset_idx(self, env_ids): 1043 | """ """ 1044 | if [body.name for body in self._scene.bodies] != [ 1045 | body.name for body in self._scene_cache.bodies 1046 | ]: 1047 | raise ValueError( 1048 | "For Isaac Gym, the list of bodies cannot be altered after the first reset" 1049 | ) 1050 | 1051 | for body in self._scene.bodies: 1052 | self._reset_base_state_buffer(body) 1053 | 1054 | if self._asset_num_dofs[body.name] == 0: 1055 | for attr in ("initial_dof_position", "initial_dof_velocity"): 1056 | if getattr(body, attr) is not None: 1057 | raise ValueError( 1058 | f"'{attr}' must be None for body with 0 DoF: '{body.name}'" 1059 | ) 1060 | else: 1061 | self._reset_dof_state_buffer(body) 1062 | 1063 | # Reset base state. 1064 | if self._actor_root_state is not None: 1065 | actor_indices = self._actor_indices[env_ids].view(-1) 1066 | if self._actor_indices_need_filter: 1067 | actor_indices = actor_indices[actor_indices != -1] 1068 | self._gym.set_actor_root_state_tensor_indexed( 1069 | self._sim, 1070 | gymtorch.unwrap_tensor(self._actor_root_state), 1071 | gymtorch.unwrap_tensor(actor_indices), 1072 | len(actor_indices), 1073 | ) 1074 | 1075 | # Reset DoF state. 1076 | if self._dof_state is not None: 1077 | actor_indices = self._actor_indices[ 1078 | env_ids[:, None], 1079 | [self._asset_num_dofs[body.name] > 0 for body in self._scene.bodies], 1080 | ].view(-1) 1081 | if self._actor_indices_need_filter: 1082 | actor_indices = actor_indices[actor_indices != -1] 1083 | self._gym.set_dof_state_tensor_indexed( 1084 | self._sim, 1085 | gymtorch.unwrap_tensor(self._dof_state), 1086 | gymtorch.unwrap_tensor(actor_indices), 1087 | len(actor_indices), 1088 | ) 1089 | 1090 | self._check_and_update_body_props(env_ids=env_ids) 1091 | 1092 | def _reset_base_state_buffer(self, body): 1093 | """ """ 1094 | if body.initial_base_position is None: 1095 | initial_base_position = self._initial_actor_root_state[ 1096 | self._actor_root_indices[body.name], :7 1097 | ] 1098 | else: 1099 | if body.env_ids_load is None or body.initial_base_position.ndim == 1: 1100 | initial_base_position = body.initial_base_position 1101 | else: 1102 | initial_base_position = body.initial_base_position[body.env_ids_load] 1103 | self._actor_root_state[self._actor_root_indices[body.name], :7] = initial_base_position 1104 | if body.initial_base_velocity is None: 1105 | initial_base_velocity = self._initial_actor_root_state[ 1106 | self._actor_root_indices[body.name], 7: 1107 | ] 1108 | else: 1109 | if body.env_ids_load is None or body.initial_base_velocity.ndim == 1: 1110 | initial_base_velocity = body.initial_base_velocity 1111 | else: 1112 | initial_base_velocity = body.initial_base_velocity[body.env_ids_load] 1113 | self._actor_root_state[self._actor_root_indices[body.name], 7:] = initial_base_velocity 1114 | 1115 | def _reset_dof_state_buffer(self, body): 1116 | """ """ 1117 | if body.initial_dof_position is None: 1118 | initial_dof_position = torch.as_strided( 1119 | self._initial_dof_state[:, 0], 1120 | ( 1121 | len(self._initial_dof_state) - self._asset_num_dofs[body.name] + 1, 1122 | self._asset_num_dofs[body.name], 1123 | ), 1124 | (2, 2), 1125 | )[self._dof_indices[body.name]] 1126 | else: 1127 | if body.env_ids_load is None or body.initial_dof_position.ndim == 1: 1128 | initial_dof_position = body.initial_dof_position 1129 | else: 1130 | initial_dof_position = body.initial_dof_position[body.env_ids_load] 1131 | torch.as_strided( 1132 | self._dof_state[:, 0], 1133 | ( 1134 | len(self._dof_state) - self._asset_num_dofs[body.name] + 1, 1135 | self._asset_num_dofs[body.name], 1136 | ), 1137 | (2, 2), 1138 | )[self._dof_indices[body.name]] = initial_dof_position 1139 | if body.initial_dof_velocity is None: 1140 | initial_dof_velocity = torch.as_strided( 1141 | self._initial_dof_state[:, 1], 1142 | ( 1143 | len(self._initial_dof_state) - self._asset_num_dofs[body.name] + 1, 1144 | self._asset_num_dofs[body.name], 1145 | ), 1146 | (2, 2), 1147 | )[self._dof_indices[body.name]] 1148 | else: 1149 | if body.env_ids_load is None or body.initial_dof_velocity.ndim == 1: 1150 | initial_dof_velocity = body.initial_dof_velocity 1151 | else: 1152 | initial_dof_velocity = body.initial_dof_velocity[body.env_ids_load] 1153 | torch.as_strided( 1154 | self._dof_state[:, 1], 1155 | ( 1156 | len(self._dof_state) - self._asset_num_dofs[body.name] + 1, 1157 | self._asset_num_dofs[body.name], 1158 | ), 1159 | (2, 2), 1160 | )[self._dof_indices[body.name]] = initial_dof_velocity 1161 | 1162 | def _check_and_update_body_props(self, env_ids=None): 1163 | """ """ 1164 | for body in self._scene.bodies: 1165 | for attr in ("scale", "link_color"): 1166 | if body.attr_array_dirty_flag[attr]: 1167 | if env_ids is not None and not np.all( 1168 | np.isin(np.nonzero(body.attr_array_dirty_mask[attr])[0], env_ids.cpu()) 1169 | ): 1170 | raise ValueError( 1171 | f"For Isaac Gym, to change '{attr}' for some env also requires the env " 1172 | f"indices to be in `env_ids`: '{body.name}'" 1173 | ) 1174 | env_ids_masked = np.nonzero(body.attr_array_dirty_mask[attr])[0] 1175 | for idx in env_ids_masked: 1176 | if attr == "scale": 1177 | self._set_scale(body, idx) 1178 | if attr == "link_color": 1179 | self._set_link_color(body, idx) 1180 | body.attr_array_dirty_flag[attr] = False 1181 | body.attr_array_dirty_mask[attr][:] = False 1182 | 1183 | if any(body.attr_array_dirty_flag[x] for x in self._ATTR_RIGID_SHAPE_PROPS): 1184 | mask = np.zeros(self._num_envs, dtype=bool) 1185 | for attr in self._ATTR_RIGID_SHAPE_PROPS: 1186 | if body.attr_array_dirty_flag[attr]: 1187 | if env_ids is not None and not np.all( 1188 | np.isin(np.nonzero(body.attr_array_dirty_mask[attr])[0], env_ids.cpu()) 1189 | ): 1190 | raise ValueError( 1191 | f"For Isaac Gym, to change '{attr}' for some env also requires the " 1192 | f"env indices to be in `env_ids`: '{body.name}'" 1193 | ) 1194 | mask |= body.attr_array_dirty_mask[attr] 1195 | env_ids_masked = np.nonzero(mask)[0] 1196 | for idx in env_ids_masked: 1197 | self._set_rigid_shape_props(body, idx) 1198 | for attr in self._ATTR_RIGID_SHAPE_PROPS: 1199 | if body.attr_array_dirty_flag[attr]: 1200 | body.attr_array_dirty_flag[attr] = False 1201 | body.attr_array_dirty_mask[attr][:] = False 1202 | 1203 | for attr in ("link_linear_damping", "link_angular_damping"): 1204 | if body.attr_array_dirty_flag[attr]: 1205 | raise ValueError( 1206 | f"For Isaac Gym, '{attr}' cannot be changed after the first reset: " 1207 | f"'{body.name}'" 1208 | ) 1209 | 1210 | if self._asset_num_dofs[body.name] > 0: 1211 | if body.attr_array_dirty_flag["dof_control_mode"]: 1212 | raise ValueError( 1213 | "For Isaac Gym, 'dof_control_mode' cannot be changed after the first " 1214 | f"reset: '{body.name}'" 1215 | ) 1216 | if any( 1217 | body.attr_array_dirty_flag[x] 1218 | for x in self._ATTR_DOF_PROPS 1219 | if x != "dof_control_mode" 1220 | ): 1221 | mask = np.zeros(self._num_envs, dtype=bool) 1222 | for attr in self._ATTR_DOF_PROPS: 1223 | if body.attr_array_dirty_flag[attr]: 1224 | if env_ids is not None and not np.all( 1225 | np.isin( 1226 | np.nonzero(body.attr_array_dirty_mask[attr])[0], env_ids.cpu() 1227 | ) 1228 | ): 1229 | raise ValueError( 1230 | f"For Isaac Gym, to change '{attr}' for certain env also " 1231 | f"requires the env index to be in `env_ids`: '{body.name}'" 1232 | ) 1233 | mask |= body.attr_array_dirty_mask[attr] 1234 | env_ids_masked = np.nonzero(mask)[0] 1235 | for idx in env_ids_masked: 1236 | self._set_dof_props(body, idx, set_drive_mode=False) 1237 | for attr in self._ATTR_DOF_PROPS: 1238 | if body.attr_array_dirty_flag[attr]: 1239 | body.attr_array_dirty_flag[attr] = False 1240 | body.attr_array_dirty_mask[attr][:] = False 1241 | else: 1242 | for attr in self._ATTR_DOF_PROPS: 1243 | if body.attr_array_dirty_flag[attr]: 1244 | raise ValueError( 1245 | f"'{attr}' must be None for body with 0 DoF: '{body.name}'" 1246 | ) 1247 | 1248 | def _clear_state(self): 1249 | """ """ 1250 | for body in self._scene.bodies: 1251 | body.dof_state = None 1252 | body.link_state = None 1253 | 1254 | self._dof_state_refreshed = False 1255 | self._link_state_refreshed = False 1256 | 1257 | def _clear_image(self): 1258 | """ """ 1259 | for camera in self._scene.cameras: 1260 | camera.color = None 1261 | camera.depth = None 1262 | camera.segmentation = None 1263 | 1264 | self._all_camera_rendered = False 1265 | 1266 | def step(self): 1267 | """ """ 1268 | if [body.name for body in self._scene.bodies] != [ 1269 | body.name for body in self._scene_cache.bodies 1270 | ]: 1271 | raise ValueError( 1272 | "For Isaac Gym, the list of bodies cannot be altered after the first reset" 1273 | ) 1274 | 1275 | self._check_and_update_body_props() 1276 | 1277 | reset_base_state = False 1278 | reset_dof_state = False 1279 | actor_indices_base = [] 1280 | actor_indices_dof = [] 1281 | 1282 | for b, body in enumerate(self._scene.bodies): 1283 | if body.env_ids_reset_base_state is not None: 1284 | if body.env_ids_load is not None and not torch.all( 1285 | torch.isin(body.env_ids_reset_base_state, body.env_ids_load) 1286 | ): 1287 | raise ValueError( 1288 | "'env_ids_reset_base_state' must be a subset of 'env_ids_load' for " 1289 | f"non-None 'env_ids_load': '{body.name}'" 1290 | ) 1291 | self._reset_base_state_buffer(body) 1292 | if not reset_base_state: 1293 | reset_base_state = True 1294 | actor_indices_base.append(self._actor_indices[body.env_ids_reset_base_state, b]) 1295 | body.env_ids_reset_base_state = None 1296 | 1297 | if self._asset_num_dofs[body.name] == 0: 1298 | for attr in ( 1299 | "dof_target_position", 1300 | "dof_target_velocity", 1301 | "dof_force", 1302 | "env_ids_reset_dof_state", 1303 | ): 1304 | if getattr(body, attr) is not None: 1305 | raise ValueError( 1306 | f"'{attr}' must be None for body with 0 DoF: '{body.name}'" 1307 | ) 1308 | continue 1309 | 1310 | if body.env_ids_reset_dof_state is not None: 1311 | if body.env_ids_load is not None and not torch.all( 1312 | torch.isin(body.env_ids_reset_dof_state, body.env_ids_load) 1313 | ): 1314 | raise ValueError( 1315 | "'env_ids_reset_dof_state' must be a subset of 'env_ids_load' for non-None " 1316 | f"'env_ids_load': '{body.name}'" 1317 | ) 1318 | self._reset_dof_state_buffer(body) 1319 | if not reset_dof_state: 1320 | reset_dof_state = True 1321 | actor_indices_dof.append(self._actor_indices[body.env_ids_reset_dof_state, b]) 1322 | body.env_ids_reset_dof_state = None 1323 | 1324 | if body.dof_target_position is not None and ( 1325 | body.dof_control_mode is None 1326 | or body.dof_control_mode.ndim == 0 1327 | and body.dof_control_mode != DoFControlMode.POSITION_CONTROL 1328 | or body.dof_control_mode.ndim == 1 1329 | and DoFControlMode.POSITION_CONTROL not in body.dof_control_mode 1330 | ): 1331 | raise ValueError( 1332 | "For Isaac Gym, 'dof_target_position' can only be set in the POSITION_CONTROL " 1333 | f"mode: '{body.name}'" 1334 | ) 1335 | if body.dof_target_velocity is not None and ( 1336 | body.dof_control_mode is None 1337 | or body.dof_control_mode.ndim == 0 1338 | and body.dof_control_mode != DoFControlMode.VELOCITY_CONTROL 1339 | or body.dof_control_mode.ndim == 1 1340 | and DoFControlMode.VELOCITY_CONTROL not in body.dof_control_mode 1341 | ): 1342 | raise ValueError( 1343 | "For Isaac Gym, 'dof_target_velocity' can only be set in the VELOCITY_CONTROL " 1344 | f"mode: '{body.name}'" 1345 | ) 1346 | if body.dof_force is not None and ( 1347 | body.dof_control_mode is None 1348 | or body.dof_control_mode.ndim == 0 1349 | and body.dof_control_mode != DoFControlMode.TORQUE_CONTROL 1350 | or body.dof_control_mode.ndim == 1 1351 | and DoFControlMode.TORQUE_CONTROL not in body.dof_control_mode 1352 | ): 1353 | raise ValueError( 1354 | "For Isaac Gym, 'dof_force' can only be set in the TORQUE_CONTROL mode: " 1355 | f"'{body.name}'" 1356 | ) 1357 | 1358 | # DriveMode is defaulted to DOF_MODE_NONE if dof_control_mode is None. 1359 | if body.dof_control_mode is None: 1360 | continue 1361 | if body.dof_control_mode.ndim == 0: 1362 | if body.dof_control_mode == DoFControlMode.POSITION_CONTROL: 1363 | if body.env_ids_load is None or body.dof_target_position.ndim == 1: 1364 | dof_target_position = body.dof_target_position 1365 | else: 1366 | dof_target_position = body.dof_target_position[body.env_ids_load] 1367 | torch.as_strided( 1368 | self._dof_position_target_buffer, 1369 | ( 1370 | len(self._dof_position_target_buffer) 1371 | - self._asset_num_dofs[body.name] 1372 | + 1, 1373 | self._asset_num_dofs[body.name], 1374 | ), 1375 | (1, 1), 1376 | )[self._dof_indices[body.name]] = dof_target_position 1377 | if body.dof_control_mode == DoFControlMode.VELOCITY_CONTROL: 1378 | if body.env_ids_load is None or body.dof_target_velocity.ndim == 1: 1379 | dof_target_velocity = body.dof_target_velocity 1380 | else: 1381 | dof_target_velocity = body.dof_target_velocity[body.env_ids_load] 1382 | torch.as_strided( 1383 | self._dof_velocity_target_buffer, 1384 | ( 1385 | len(self._dof_velocity_target_buffer) 1386 | - self._asset_num_dofs[body.name] 1387 | + 1, 1388 | self._asset_num_dofs[body.name], 1389 | ), 1390 | (1, 1), 1391 | )[self._dof_indices[body.name]] = dof_target_velocity 1392 | if body.dof_control_mode == DoFControlMode.TORQUE_CONTROL: 1393 | if body.env_ids_load is None or body.dof_force.ndim == 1: 1394 | dof_force = body.dof_force 1395 | else: 1396 | dof_force = body.dof_force[body.env_ids_load] 1397 | torch.as_strided( 1398 | self._dof_actuation_force_buffer, 1399 | ( 1400 | len(self._dof_actuation_force_buffer) 1401 | - self._asset_num_dofs[body.name] 1402 | + 1, 1403 | self._asset_num_dofs[body.name], 1404 | ), 1405 | (1, 1), 1406 | )[self._dof_indices[body.name]] = dof_force 1407 | if body.dof_control_mode.ndim == 1: 1408 | if DoFControlMode.POSITION_CONTROL in body.dof_control_mode: 1409 | if body.env_ids_load is None or body.dof_target_position.ndim == 1: 1410 | dof_target_position = body.dof_target_position[ 1411 | ..., body.dof_control_mode == DoFControlMode.POSITION_CONTROL 1412 | ] 1413 | else: 1414 | dof_target_position = body.dof_target_position[ 1415 | body.env_ids_load[:, None], 1416 | body.dof_control_mode == DoFControlMode.POSITION_CONTROL, 1417 | ] 1418 | torch.as_strided( 1419 | self._dof_position_target_buffer, 1420 | ( 1421 | len(self._dof_position_target_buffer) 1422 | - self._asset_num_dofs[body.name] 1423 | + 1, 1424 | self._asset_num_dofs[body.name], 1425 | ), 1426 | (1, 1), 1427 | )[ 1428 | self._dof_indices[body.name][:, None], 1429 | body.dof_control_mode == DoFControlMode.POSITION_CONTROL, 1430 | ] = dof_target_position 1431 | if DoFControlMode.VELOCITY_CONTROL in body.dof_control_mode: 1432 | if body.env_ids_load is None or body.dof_target_position.ndim == 1: 1433 | dof_target_velocity = body.dof_target_velocity[ 1434 | ..., body.dof_control_mode == DoFControlMode.VELOCITY_CONTROL 1435 | ] 1436 | else: 1437 | dof_target_velocity = body.dof_target_velocity[ 1438 | body.env_ids_load[:, None], 1439 | body.dof_control_mode == DoFControlMode.VELOCITY_CONTROL, 1440 | ] 1441 | torch.as_strided( 1442 | self._dof_velocity_target_buffer, 1443 | ( 1444 | len(self._dof_velocity_target_buffer) 1445 | - self._asset_num_dofs[body.name] 1446 | + 1, 1447 | self._asset_num_dofs[body.name], 1448 | ), 1449 | (1, 1), 1450 | )[ 1451 | self._dof_indices[body.name][:, None], 1452 | body.dof_control_mode == DoFControlMode.VELOCITY_CONTROL, 1453 | ] = dof_target_velocity 1454 | if DoFControlMode.TORQUE_CONTROL in body.dof_control_mode: 1455 | if body.env_ids_load is None or body.dof_force.ndim == 1: 1456 | dof_force = body.dof_force[ 1457 | ..., body.dof_control_mode == DoFControlMode.TORQUE_CONTROL 1458 | ] 1459 | else: 1460 | dof_force = body.dof_force[ 1461 | body.env_ids_load[:, None], 1462 | body.dof_control_mode == DoFControlMode.TORQUE_CONTROL, 1463 | ] 1464 | torch.as_strided( 1465 | self._dof_actuation_force_buffer, 1466 | ( 1467 | len(self._dof_actuation_force_buffer) 1468 | - self._asset_num_dofs[body.name] 1469 | + 1, 1470 | self._asset_num_dofs[body.name], 1471 | ), 1472 | (1, 1), 1473 | )[ 1474 | self._dof_indices[body.name][:, None], 1475 | body.dof_control_mode == DoFControlMode.TORQUE_CONTROL, 1476 | ] = dof_force 1477 | 1478 | if reset_base_state: 1479 | actor_indices = torch.cat(actor_indices_base) 1480 | self._gym.set_actor_root_state_tensor_indexed( 1481 | self._sim, 1482 | gymtorch.unwrap_tensor(self._actor_root_state), 1483 | gymtorch.unwrap_tensor(actor_indices), 1484 | len(actor_indices), 1485 | ) 1486 | 1487 | if reset_dof_state: 1488 | actor_indices = torch.cat(actor_indices_dof) 1489 | self._gym.set_dof_state_tensor_indexed( 1490 | self._sim, 1491 | gymtorch.unwrap_tensor(self._dof_state), 1492 | gymtorch.unwrap_tensor(actor_indices), 1493 | len(actor_indices), 1494 | ) 1495 | 1496 | if self._dof_state is not None: 1497 | self._gym.set_dof_position_target_tensor( 1498 | self._sim, gymtorch.unwrap_tensor(self._dof_position_target_buffer) 1499 | ) 1500 | self._gym.set_dof_velocity_target_tensor( 1501 | self._sim, gymtorch.unwrap_tensor(self._dof_velocity_target_buffer) 1502 | ) 1503 | self._gym.set_dof_actuation_force_tensor( 1504 | self._sim, gymtorch.unwrap_tensor(self._dof_actuation_force_buffer) 1505 | ) 1506 | 1507 | self._gym.simulate(self._sim) 1508 | if self._device == "cpu" or self._cfg.RENDER or self._cfg.ISAAC_GYM.ENABLE_CAMERA_SENSORS: 1509 | self._gym.fetch_results(self._sim, True) 1510 | 1511 | self._graphics_stepped = False 1512 | self._clear_state() 1513 | self._clear_image() 1514 | self._contact = None 1515 | 1516 | if self._viewer: 1517 | if self._gym.query_viewer_has_closed(self._viewer): 1518 | sys.exit() 1519 | 1520 | for evt in self._gym.query_viewer_action_events(self._viewer): 1521 | if evt.action == "quit" and evt.value > 0: 1522 | sys.exit() 1523 | if evt.action == "toggle_viewer_sync" and evt.value > 0: 1524 | self._enable_viewer_sync = not self._enable_viewer_sync 1525 | 1526 | if self._enable_viewer_sync: 1527 | if (self._counter_render % self._render_steps) <= ( 1528 | self._counter_render - 1 1529 | ) % self._render_steps: 1530 | # Simulate real-time rendering with sleep if computation takes less than real time. 1531 | time_spent = time.time() - self._last_render_time 1532 | time_sleep = self._render_time_step - time_spent 1533 | if time_sleep > 0: 1534 | time.sleep(time_sleep) 1535 | self._last_render_time = time.time() 1536 | 1537 | self._check_and_step_graphics() 1538 | 1539 | self._gym.draw_viewer(self._viewer, self._sim) 1540 | 1541 | self._counter_render += 1 1542 | else: 1543 | self._gym.poll_viewer_events(self._viewer) 1544 | 1545 | if self._counter_render != 0: 1546 | self._counter_render = 0 1547 | 1548 | @property 1549 | def contact(self): 1550 | """ """ 1551 | if self._contact is None: 1552 | self._contact = self._collect_contact() 1553 | return self._contact 1554 | 1555 | def _collect_contact(self): 1556 | """ """ 1557 | contact = [] 1558 | for env in self._envs: 1559 | rigid_contacts = self._gym.get_env_rigid_contacts(env) 1560 | if len(rigid_contacts) == 0: 1561 | contact_array = create_contact_array(0) 1562 | else: 1563 | kwargs = {} 1564 | kwargs["body_id_a"], kwargs["link_id_a"] = zip( 1565 | *[self._asset_rigid_body_mapping[x] for x in rigid_contacts["body0"]] 1566 | ) 1567 | kwargs["body_id_b"], kwargs["link_id_b"] = zip( 1568 | *[self._asset_rigid_body_mapping[x] for x in rigid_contacts["body1"]] 1569 | ) 1570 | kwargs["position_a_world"] = np.nan 1571 | kwargs["position_b_world"] = np.nan 1572 | kwargs["position_a_link"] = rigid_contacts["localPos0"] 1573 | kwargs["position_b_link"] = rigid_contacts["localPos1"] 1574 | kwargs["normal"] = rigid_contacts["normal"] 1575 | kwargs["force"] = rigid_contacts["lambda"] 1576 | contact_array = create_contact_array(len(rigid_contacts), **kwargs) 1577 | contact.append(contact_array) 1578 | return contact 1579 | 1580 | def close(self): 1581 | """ """ 1582 | if self._created: 1583 | for idx in range(self._num_envs): 1584 | for name in self._camera_handles[idx]: 1585 | self._gym.destroy_camera_sensor( 1586 | self._sim, self._envs[idx], self._camera_handles[idx][name] 1587 | ) 1588 | self._gym.destroy_viewer(self._viewer) 1589 | self._gym.destroy_sim(self._sim) 1590 | self._created = False 1591 | -------------------------------------------------------------------------------- /src/easysim/simulators/registration.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import importlib 6 | 7 | 8 | def load(entry_point): 9 | """ """ 10 | mod_name, attr_name = entry_point.split(":") 11 | mod = importlib.import_module(mod_name) 12 | fn = getattr(mod, attr_name) 13 | return fn 14 | 15 | 16 | class SimulatorSpec: 17 | """ """ 18 | 19 | def __init__(self, name, entry_point, kwargs=None): 20 | """ """ 21 | self._name = name 22 | self._entry_point = entry_point 23 | self._kwargs = {} if kwargs is None else kwargs 24 | 25 | def make(self, **kwargs): 26 | """ """ 27 | _kwargs = self._kwargs.copy() 28 | _kwargs.update(kwargs) 29 | 30 | fn = load(self._entry_point) 31 | simulator = fn(**_kwargs) 32 | 33 | return simulator 34 | 35 | 36 | class SimulatorRegistry: 37 | """ """ 38 | 39 | def __init__(self): 40 | """ """ 41 | self._specs = {} 42 | 43 | def register(self, name, **kwargs): 44 | """ """ 45 | if name in self._specs: 46 | raise Exception(f"Cannot re-register name: '{name}'") 47 | self._specs[name] = SimulatorSpec(name, **kwargs) 48 | 49 | def make(self, name, **kwargs): 50 | """ """ 51 | if name not in self._specs: 52 | raise KeyError(f"No registered simulator with name: '{name}'") 53 | spec = self._specs[name] 54 | 55 | simulator = spec.make(**kwargs) 56 | 57 | return simulator 58 | 59 | 60 | registry = SimulatorRegistry() 61 | 62 | 63 | def register(name, **kwargs): 64 | """ """ 65 | registry.register(name, **kwargs) 66 | 67 | 68 | def make(name, **kwargs): 69 | """ """ 70 | return registry.make(name, **kwargs) 71 | 72 | 73 | register( 74 | name="bullet", 75 | entry_point="easysim.simulators.bullet:Bullet", 76 | ) 77 | 78 | register( 79 | name="isaac_gym", 80 | entry_point="easysim.simulators.isaac_gym:IsaacGym", 81 | ) 82 | -------------------------------------------------------------------------------- /src/easysim/simulators/simulator.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | import abc 6 | 7 | 8 | class Simulator(abc.ABC): 9 | """Simulator.""" 10 | 11 | def __init__(self, cfg, scene): 12 | """ """ 13 | self._cfg = cfg 14 | self._scene = scene 15 | 16 | @abc.abstractmethod 17 | def reset(self, env_ids): 18 | """ """ 19 | 20 | @abc.abstractmethod 21 | def step(self): 22 | """ """ 23 | 24 | @property 25 | @abc.abstractmethod 26 | def contact(self): 27 | """ """ 28 | 29 | @abc.abstractmethod 30 | def close(self): 31 | """ """ 32 | -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | """EasySim test package.""" 6 | -------------------------------------------------------------------------------- /tests/easysim_version_test.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # 3 | # Licensed under the MIT License [see LICENSE for details]. 4 | 5 | """Unit tests for the `easysim` package version.""" 6 | 7 | # SRL 8 | import easysim 9 | 10 | 11 | def test_easysim_version() -> None: 12 | """Test `easysim` package version is set.""" 13 | assert easysim.__version__ is not None 14 | assert easysim.__version__ != "" 15 | --------------------------------------------------------------------------------