Unverified Commit 24782318 authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub

Adds utilities for rough terrain generation (#20)

# Description

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link: https://isaac-orbit.github.io/orbit/source/refs/contributing.html
-->

This PR ports in the functions for procedurally generating different
terrains. The terrains can be generated as heightfields (2.5 D elevation
map) or meshes (through `trimesh`).

There are two separate classes:
* `TerrainGenerator`: This class only procedurally generates terrains
based on the passed sub-terrain configuration. It creates the trimesh
object and contains the origins of each terrain in the mesh.
Additionally, it performs caching of the sub-terrain meshes based on
their configuration. By default, caching is disabled since most
developers will change not only the config, but also the function.
* `TerrainImporter`: This class mainly deals with importing terrains
from different possible sources and adding them to the simulator. In
addition to importing terrains, it also stores them into a dictionary
called `warp_meshes` that later can be used for ray-casting. The types
of terrain sources are as follows:
* `import_ground_plane`: creates a grid plane which is default in
isaacsim/orbit.
    * `import_mesh`: takes a trimesh object
    * `import_usd`: takes a USD file and adds reference to it

<!-- As a practice, it is recommended to open an issue to have
discussions on the proposed pull request.
This makes it easier for the community to keep track of what is being
developed or added, and if a given feature
is demanded by more than one party. -->

## Type of change

<!-- As you go through the list, delete the ones that are not
applicable. -->

- New feature (non-breaking change which adds functionality)

## Screenshots


![terrains](https://github.com/isaac-orbit/orbit/assets/12863862/05f889d0-b98e-4063-a810-89d9116c2dfe)

<!--
Example:

| Before | After |
| ------ | ----- |
| _gif/png before_ | _gif/png after_ |

To upload images to a PR -- simply drag and drop an image while in edit
mode and it should upload the image directly. You can then paste that
source into the above before/after sections.
-->

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->
parent cd54c89d
...@@ -43,7 +43,13 @@ ...@@ -43,7 +43,13 @@
"flatcache", "flatcache",
"physx", "physx",
"dpad", "dpad",
"gamepad" "gamepad",
"linspace",
"upsampled",
"downsampled",
"arange",
"discretization",
"trimesh"
], ],
// This enables python language server. Seems to work slightly better than jedi: // This enables python language server. Seems to work slightly better than jedi:
"python.languageServer": "Pylance", "python.languageServer": "Pylance",
......
...@@ -124,6 +124,7 @@ autodoc_mock_imports = [ ...@@ -124,6 +124,7 @@ autodoc_mock_imports = [
"hid", "hid",
"prettytable", "prettytable",
"tqdm", "tqdm",
"trimesh",
"toml", "toml",
] ]
...@@ -179,3 +180,17 @@ html_theme_options = { ...@@ -179,3 +180,17 @@ html_theme_options = {
html_show_copyright = True html_show_copyright = True
html_show_sphinx = False html_show_sphinx = False
# -- Advanced configuration -------------------------------------------------
def skip_member(app, what, name, obj, skip, options):
exclusions = ["from_dict", "to_dict"] # List the names of the functions you want to skip here
if name in exclusions:
return True
return None
def setup(app):
app.connect("autodoc-skip-member", skip_member)
...@@ -17,6 +17,7 @@ omni.isaac.orbit extension ...@@ -17,6 +17,7 @@ omni.isaac.orbit extension
orbit.objects.rigid orbit.objects.rigid
orbit.robots orbit.robots
orbit.sensors orbit.sensors
orbit.terrains
orbit.utils orbit.utils
orbit.utils.assets orbit.utils.assets
orbit.utils.kit orbit.utils.kit
......
omni.isaac.orbit.terrains
=========================
.. automodule:: omni.isaac.orbit.terrains
:members:
:undoc-members:
:show-inheritance:
Terrain manager
---------------
.. autoclass:: omni.isaac.orbit.terrains.terrain_cfg.TerrainManagerCfg
:members:
:show-inheritance:
.. autoclass:: omni.isaac.orbit.terrains.terrain_manager.TerrainManager
:members:
:show-inheritance:
Terrain generator
-----------------
.. autoclass:: omni.isaac.orbit.terrains.terrain_cfg.SubTerrainBaseCfg
:members:
:show-inheritance:
.. autoclass:: omni.isaac.orbit.terrains.terrain_cfg.TerrainGeneratorCfg
:members:
:show-inheritance:
.. autoclass:: omni.isaac.orbit.terrains.terrain_generator.TerrainGenerator
:members:
:show-inheritance:
Height fields
-------------
.. automodule:: omni.isaac.orbit.terrains.height_field
Random Uniform Terrain
^^^^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.height_field.hf_terrains.random_uniform_terrain
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfRandomUniformTerrainCfg
:members:
:show-inheritance:
Pyramid Sloped Terrain
^^^^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.height_field.hf_terrains.pyramid_sloped_terrain
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfPyramidSlopedTerrainCfg
:members:
:show-inheritance:
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfInvertedPyramidSlopedTerrainCfg
:members:
:show-inheritance:
Pyramid Stairs Terrain
^^^^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.height_field.hf_terrains.pyramid_stairs_terrain
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfPyramidStairsTerrainCfg
:members:
:show-inheritance:
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfInvertedPyramidStairsTerrainCfg
:members:
:show-inheritance:
Discrete Obstacles Terrain
^^^^^^^^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.height_field.hf_terrains.discrete_obstacles_terrain
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfDiscreteObstaclesTerrainCfg
:members:
:show-inheritance:
Wave Terrain
^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.height_field.hf_terrains.wave_terrain
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfWaveTerrainCfg
:members:
:show-inheritance:
Stepping Stones Terrain
^^^^^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.height_field.hf_terrains.stepping_stones_terrain
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfSteppingStonesTerrainCfg
:members:
:show-inheritance:
Trimesh terrains
----------------
.. automodule:: omni.isaac.orbit.terrains.trimesh
Flat terrain
^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.flat_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshPlaneTerrainCfg
:members:
:show-inheritance:
Pyramid terrain
^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.pyramid_stairs_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshPyramidStairsTerrainCfg
:members:
:show-inheritance:
Inverted pyramid terrain
^^^^^^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.inverted_pyramid_stairs_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshInvertedPyramidStairsTerrainCfg
:members:
:show-inheritance:
Random grid terrain
^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.random_grid_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshRandomGridTerrainCfg
:members:
:show-inheritance:
Rails terrain
^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.rails_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshRailsTerrainCfg
:members:
:show-inheritance:
Pit terrain
^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.pit_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshPitTerrainCfg
:members:
:show-inheritance:
Box terrain
^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.box_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshBoxTerrainCfg
:members:
:show-inheritance:
Gap terrain
^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.gap_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshGapTerrainCfg
:members:
:show-inheritance:
Floating ring terrain
^^^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.floating_ring_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshFloatingRingTerrainCfg
:members:
:show-inheritance:
Star terrain
^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.star_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshStarTerrainCfg
:members:
:show-inheritance:
Repeated Objects Terrain
^^^^^^^^^^^^^^^^^^^^^^^^
.. autofunction:: omni.isaac.orbit.terrains.trimesh.mesh_terrains.repeated_objects_terrain
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshRepeatedObjectsTerrainCfg
:members:
:show-inheritance:
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshRepeatedPyramidsTerrainCfg
:members:
:show-inheritance:
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshRepeatedBoxesTerrainCfg
:members:
:show-inheritance:
.. autoclass:: omni.isaac.orbit.terrains.trimesh.mesh_terrains_cfg.MeshRepeatedCylindersTerrainCfg
:members:
:show-inheritance:
Utilities
---------
.. automodule:: omni.isaac.orbit.terrains.utils
:members:
:undoc-members:
...@@ -32,6 +32,8 @@ extra_standard_library = [ ...@@ -32,6 +32,8 @@ extra_standard_library = [
"yaml", "yaml",
"prettytable", "prettytable",
"toml", "toml",
"trimesh",
"tqdm",
] ]
# Imports from Isaac Sim and Omniverse # Imports from Isaac Sim and Omniverse
known_third_party = [ known_third_party = [
......
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.4.1" version = "0.4.2"
# Description # Description
title = "ORBIT framework for Robot Learning" title = "ORBIT framework for Robot Learning"
......
Changelog Changelog
--------- ---------
0.4.1 (2023-06-27) 0.4.2 (2023-06-28)
~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~
Added Added
^^^^^ ^^^^^
* Added the sub-module :mod:`omni.isaac.orbit.terrains` to allow procedural generation of terrains and supporting
importing of terrains from different sources (meshes, usd files or default ground plane).
0.4.1 (2023-06-27)
~~~~~~~~~~~~~~~~~~
* Added the :class:`omni.isaac.orbit.app.AppLauncher` class to allow controlled instantiation of * Added the :class:`omni.isaac.orbit.app.AppLauncher` class to allow controlled instantiation of
the `SimulationApp <https://docs.omniverse.nvidia.com/py/isaacsim/source/extensions/omni.isaac.kit/docs/index.html>`_ the `SimulationApp <https://docs.omniverse.nvidia.com/py/isaacsim/source/extensions/omni.isaac.kit/docs/index.html>`_
and extension loading for remote deployment and ROS bridges. and extension loading for remote deployment and ROS bridges.
......
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This module provides utilities to create different terrains procedurally.
There are two main components in this module:
* :class:`TerrainGenerator`: This class procedurally generates terrains based on the passed
sub-terrain configuration. It creates a ``trimesh`` mesh object and contains the origins of
each generated sub-terrain.
* :class:`TerrainImporter`: This class mainly deals with importing terrains from different
possible sources and adding them to the simulator as a prim object. It also stores the
terrain mesh into a dictionary called :obj:`warp_meshes` that later can be used
for ray-casting. The following functions are available for importing terrains:
* :meth:`import_ground_plane`: spawn a grid plane which is default in isaacsim/orbit.
* :meth:`import_mesh`: spawn a prim from a ``trimesh`` object.
* :meth:`import_usd`: spawn a prim as reference to input USD file.
"""
from .height_field import * # noqa: F401, F403
from .terrain_cfg import SubTerrainBaseCfg, TerrainGeneratorCfg, TerrainImporterCfg
from .terrain_generator import TerrainGenerator
from .terrain_importer import TerrainImporter
from .trimesh import * # noqa: F401, F403
from .utils import color_meshes_by_height, convert_to_warp_mesh, create_prim_from_mesh
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Configuration for custom terrains."""
import omni.isaac.orbit.terrains as terrain_gen
from ..terrain_cfg import TerrainGeneratorCfg
ASSORTED_TERRAINS_CFG = TerrainGeneratorCfg(
size=(8.0, 8.0),
border_width=20.0,
num_rows=10,
num_cols=20,
horizontal_scale=0.1,
vertical_scale=0.005,
slope_threshold=0.75,
difficulty_choices=(0.5, 0.75, 0.9),
use_cache=False,
sub_terrains={
"pyramid_stairs": terrain_gen.MeshPyramidStairsTerrainCfg(
proportion=0.2,
step_height_range=(0.05, 0.23),
step_width=0.3,
platform_width=3.0,
border_width=1.0,
holes=False,
),
"pyramid_stairs_inv": terrain_gen.MeshInvertedPyramidStairsTerrainCfg(
proportion=0.2,
step_height_range=(0.05, 0.23),
step_width=0.3,
platform_width=3.0,
border_width=1.0,
holes=False,
),
"boxes": terrain_gen.MeshRandomGridTerrainCfg(
proportion=0.2, grid_width=0.45, grid_height_range=(0.05, 0.2), platform_width=2.0
),
"random_rough": terrain_gen.HfRandomUniformTerrainCfg(
proportion=0.2, noise_range=(0.02, 0.10), noise_step=0.02, border_width=0.25
),
"hf_pyramid_slope": terrain_gen.HfPyramidSlopedTerrainCfg(
proportion=0.1, slope_range=(0.0, 0.4), platform_width=2.0, border_width=0.25
),
"hf_pyramid_slope_inv": terrain_gen.HfInvertedPyramidSlopedTerrainCfg(
proportion=0.1, slope_range=(0.0, 0.4), platform_width=2.0, border_width=0.25
),
},
)
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This sub-module provides utilities to create different terrains as height fields (HF).
Height fields are a 2.5D terrain representation that is used in robotics to obtain the
height of the terrain at a given point. This is useful for controls and planning algorithms.
Each terrain is represented as a 2D numpy array with discretized heights. The shape of the array
is (width, length), where width and length are the number of points along the x and y axis,
respectively. The height of the terrain at a given point is obtained by indexing the array with
the corresponding x and y coordinates.
.. caution::
When working with height field terrains, it is important to remember that the terrain is generated
from a discretized 3D representation. This means that the height of the terrain at a given point
is only an approximation of the real height of the terrain at that point. The discretization
error is proportional to the size of the discretization cells. Therefore, it is important to
choose a discretization size that is small enough for the application. A larger discretization
size will result in a faster simulation, but the terrain will be less accurate.
All sub-terrains must inherit from the :class:`HfTerrainBaseCfg` class which contains the common
parameters for all terrains generated from height fields.
.. autoclass:: omni.isaac.orbit.terrains.height_field.hf_terrains_cfg.HfTerrainBaseCfg
:members:
:show-inheritance:
"""
from .hf_terrains_cfg import (
HfDiscreteObstaclesTerrainCfg,
HfInvertedPyramidSlopedTerrainCfg,
HfInvertedPyramidStairsTerrainCfg,
HfPyramidSlopedTerrainCfg,
HfPyramidStairsTerrainCfg,
HfRandomUniformTerrainCfg,
HfSteppingStonesTerrainCfg,
HfTerrainBaseCfg,
HfWaveTerrainCfg,
)
__all__ = [
"HfTerrainBaseCfg",
"HfRandomUniformTerrainCfg",
"HfPyramidSlopedTerrainCfg",
"HfInvertedPyramidSlopedTerrainCfg",
"HfPyramidStairsTerrainCfg",
"HfInvertedPyramidStairsTerrainCfg",
"HfDiscreteObstaclesTerrainCfg",
"HfWaveTerrainCfg",
"HfSteppingStonesTerrainCfg",
]
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from dataclasses import MISSING
from typing import Optional, Tuple
import omni.isaac.orbit.terrains.height_field.hf_terrains as hf_terrains
from omni.isaac.orbit.terrains.terrain_cfg import SubTerrainBaseCfg
from omni.isaac.orbit.utils import configclass
@configclass
class HfTerrainBaseCfg(SubTerrainBaseCfg):
"""The base configuration for height field terrains."""
border_width: float = 0.0
"""The width of the border/padding around the terrain (in m). Defaults to 0.0.
The border width is subtracted from the :obj:`size` of the terrain. If non-zero, it must be
greater than or equal to the :obj:`horizontal scale`.
"""
horizontal_scale: float = 0.1
"""The discretization of the terrain along the x and y axes (in m). Defaults to 0.1."""
vertical_scale: float = 0.005
"""The discretization of the terrain along the z axis (in m). Defaults to 0.005."""
slope_threshold: Optional[float] = None
"""The slope threshold above which surfaces are made vertical. Defaults to :obj:`None`.
If :obj:`None` no correction is applied.
"""
"""
Different height field terrain configurations.
"""
@configclass
class HfRandomUniformTerrainCfg(HfTerrainBaseCfg):
"""Configuration for a random uniform height field terrain."""
function = hf_terrains.random_uniform_terrain
noise_range: Tuple[float, float] = MISSING
"""The minimum and maximum height noise (i.e. along z) of the terrain (in m)."""
noise_step: float = MISSING
"""The minimum height (in m) change between two points."""
downsampled_scale: Optional[float] = None
"""The distance between two randomly sampled points on the terrain. Defaults to None,
in which case the :obj:`horizontal scale` is used.
The heights are sampled at this resolution and interpolation is performed for intermediate points.
This must be larger than or equal to the :obj:`horizontal scale`.
"""
@configclass
class HfPyramidSlopedTerrainCfg(HfTerrainBaseCfg):
"""Configuration for a pyramid sloped height field terrain."""
function = hf_terrains.pyramid_sloped_terrain
slope_range: Tuple[float, float] = MISSING
"""The slope of the terrain (in radians)."""
platform_width: float = 1.0
"""The width of the square platform at the center of the terrain. Defaults to 1.0."""
inverted: bool = False
"""Whether the pyramid is inverted. Defaults to False.
If True, the terrain is inverted such that the platform is at the bottom and the slopes are upwards.
"""
@configclass
class HfInvertedPyramidSlopedTerrainCfg(HfPyramidSlopedTerrainCfg):
"""Configuration for an inverted pyramid sloped height field terrain.
Note:
This is a subclass of :class:`HfPyramidSlopedTerrainCfg` with :obj:`inverted` set to True.
We make it as a separate class to make it easier to distinguish between the two and match
the naming convention of the other terrains.
"""
inverted: bool = True
@configclass
class HfPyramidStairsTerrainCfg(HfTerrainBaseCfg):
"""Configuration for a pyramid stairs height field terrain."""
function = hf_terrains.pyramid_stairs_terrain
step_height_range: Tuple[float, float] = MISSING
"""The minimum and maximum height of the steps (in m)."""
step_width: float = MISSING
"""The width of the steps (in m)."""
platform_width: float = 1.0
"""The width of the square platform at the center of the terrain. Defaults to 1.0."""
inverted: bool = False
"""Whether the pyramid stairs is inverted. Defaults to False.
If True, the terrain is inverted such that the platform is at the bottom and the stairs are upwards.
"""
@configclass
class HfInvertedPyramidStairsTerrainCfg(HfPyramidStairsTerrainCfg):
"""Configuration for an inverted pyramid stairs height field terrain.
Note:
This is a subclass of :class:`HfPyramidStairsTerrainCfg` with :obj:`inverted` set to True.
We make it as a separate class to make it easier to distinguish between the two and match
the naming convention of the other terrains.
"""
inverted: bool = True
@configclass
class HfDiscreteObstaclesTerrainCfg(HfTerrainBaseCfg):
"""Configuration for a discrete obstacles height field terrain."""
function = hf_terrains.discrete_obstacles_terrain
obstacle_height_mode: str = "choice"
"""The mode to use for the obstacle height. Defaults to "choice".
The following modes are supported: "choice", "fixed".
"""
obstacle_width_range: Tuple[float, float] = MISSING
"""The minimum and maximum width of the obstacles (in m)."""
obstacle_height_range: Tuple[float, float] = MISSING
"""The minimum and maximum height of the obstacles (in m)."""
num_obstacles: int = MISSING
"""The number of obstacles to generate."""
platform_width: float = 1.0
"""The width of the square platform at the center of the terrain. Defaults to 1.0."""
@configclass
class HfWaveTerrainCfg(HfTerrainBaseCfg):
"""Configuration for a wave height field terrain."""
function = hf_terrains.wave_terrain
amplitude_range: Tuple[float, float] = MISSING
"""The minimum and maximum amplitude of the wave (in m)."""
num_waves: int = 1.0
"""The number of waves to generate. Defaults to 1.0."""
@configclass
class HfSteppingStonesTerrainCfg(HfTerrainBaseCfg):
"""Configuration for a stepping stones height field terrain."""
function = hf_terrains.stepping_stones_terrain
stone_height_max: float = MISSING
"""The maximum height of the stones (in m)."""
stone_width_range: Tuple[float, float] = MISSING
"""The minimum and maximum width of the stones (in m)."""
stone_distance_range: Tuple[float, float] = MISSING
"""The minimum and maximum distance between stones (in m)."""
holes_depth: float = -10.0
"""The depth of the holes (negative obstacles). Defaults to -10.0."""
platform_width: float = 1.0
"""The width of the square platform at the center of the terrain. Defaults to 1.0."""
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from __future__ import annotations
import copy
import functools
import numpy as np
import trimesh
from typing import TYPE_CHECKING, Callable
if TYPE_CHECKING:
from .hf_terrains_cfg import HfTerrainBaseCfg
def height_field_to_mesh(func: Callable) -> Callable:
"""Decorator to convert a height field function to a mesh function.
This decorator converts a height field function to a mesh function by sampling the heights
at a specified resolution and performing interpolation to obtain the intermediate heights.
Additionally, it adds a border around the terrain to avoid artifacts at the edges.
Args:
func (Callable): The height field function to convert. The function should return a 2D
numpy array with the heights of the terrain.
Returns:
Callable: The mesh function. The mesh function returns a tuple containing a list of ``trimesh``
mesh objects and the origin of the terrain.
"""
@functools.wraps(func)
def wrapper(difficulty: float, cfg: HfTerrainBaseCfg):
# check valid border width
if cfg.border_width > 0 and cfg.border_width < cfg.horizontal_scale:
raise ValueError(
f"The border width ({cfg.border_width}) must be greater than or equal to the "
f"horizontal scale ({cfg.horizontal_scale})."
)
# allocate buffer for height field (with border)
width_pixels = int(cfg.size[0] / cfg.horizontal_scale) + 1
length_pixels = int(cfg.size[1] / cfg.horizontal_scale) + 1
border_pixels = int(cfg.border_width / cfg.horizontal_scale) + 1
heights = np.zeros((width_pixels, length_pixels), dtype=np.int16)
# override size of the terrain to account for the border
sub_terrain_size = [width_pixels - 2 * border_pixels, length_pixels - 2 * border_pixels]
sub_terrain_size = [dim * cfg.horizontal_scale for dim in sub_terrain_size]
# update the config
terrain_size = copy.deepcopy(cfg.size)
cfg.size = tuple(sub_terrain_size)
# generate the height field
z_gen = func(difficulty, cfg)
# handle the border for the terrain
heights[border_pixels:-border_pixels, border_pixels:-border_pixels] = z_gen
# set terrain size back to config
cfg.size = terrain_size
# convert to trimesh
vertices, triangles = convert_height_field_to_mesh(
heights, cfg.horizontal_scale, cfg.vertical_scale, cfg.slope_threshold
)
mesh = trimesh.Trimesh(vertices=vertices, faces=triangles)
# compute origin
x1 = int((cfg.size[0] * 0.5 - 1) / cfg.horizontal_scale)
x2 = int((cfg.size[0] * 0.5 + 1) / cfg.horizontal_scale)
y1 = int((cfg.size[1] * 0.5 - 1) / cfg.horizontal_scale)
y2 = int((cfg.size[1] * 0.5 + 1) / cfg.horizontal_scale)
origin_z = np.max(heights[x1:x2, y1:y2]) * cfg.vertical_scale
origin = np.array([0.5 * cfg.size[0], 0.5 * cfg.size[1], origin_z])
# return mesh and origin
return [mesh], origin
return wrapper
def convert_height_field_to_mesh(
height_field: np.ndarray, horizontal_scale: float, vertical_scale: float, slope_threshold: float | None = None
) -> tuple[np.ndarray, np.ndarray]:
"""Convert a height-field array to a triangle mesh represented by vertices and triangles.
This function converts a height-field array to a triangle mesh represented by vertices and triangles.
The height-field array is assumed to be a 2D array of floats, where each element represents the height
of the terrain at that location. The height-field array is assumed to be in the form of a matrix, where
the first dimension represents the x-axis and the second dimension represents the y-axis.
The function can also correct vertical surfaces above the provide slope threshold. This is helpful to
avoid having long vertical surfaces in the mesh. The correction is done by moving the vertices of the
vertical surfaces to minimum of the two neighboring vertices.
The correction is done in the following way:
If :math:`\\frac{y_2 - y_1}{x_2 - x_1} > threshold`, then move A to A' (i.e., set :math:`x_1' = x_2`).
This is repeated along all directions.
.. code-block:: none
B(x_2,y_2)
/|
/ |
/ |
(x_1,y_1)A---A'(x_1',y_1)
Args:
height_field (np.ndarray): The input height-field array.
horizontal_scale (float): The discretization of the terrain along the x and y axis.
vertical_scale (float): The discretization of the terrain along the z axis.
slope_threshold (Optional[float], optional): The slope threshold above which surfaces
are made vertical. If :obj:`None` no correction is applied. Defaults to :obj:`None`.
Returns:
Tuple[np.ndarray, np.ndarray]: The vertices and triangles of the mesh:
- **vertices** (np.ndarray(float)): Array of shape (num_vertices, 3).
Each row represents the location of each vertex (in m).
- **triangles** (np.ndarray(int)): Array of shape (num_triangles, 3).
Each row represents the indices of the 3 vertices connected by this triangle.
"""
# read height field
num_rows, num_cols = height_field.shape
# create a mesh grid of the height field
y = np.linspace(0, (num_cols - 1) * horizontal_scale, num_cols)
x = np.linspace(0, (num_rows - 1) * horizontal_scale, num_rows)
yy, xx = np.meshgrid(y, x)
# copy height field to avoid modifying the original array
hf = height_field.copy()
# correct vertical surfaces above the slope threshold
if slope_threshold is not None:
# scale slope threshold based on the horizontal and vertical scale
slope_threshold *= horizontal_scale / vertical_scale
# allocate arrays to store the movement of the vertices
move_x = np.zeros((num_rows, num_cols))
move_y = np.zeros((num_rows, num_cols))
move_corners = np.zeros((num_rows, num_cols))
# move vertices along the x-axis
move_x[: num_rows - 1, :] += hf[1:num_rows, :] - hf[: num_rows - 1, :] > slope_threshold
move_x[1:num_rows, :] -= hf[: num_rows - 1, :] - hf[1:num_rows, :] > slope_threshold
# move vertices along the y-axis
move_y[:, : num_cols - 1] += hf[:, 1:num_cols] - hf[:, : num_cols - 1] > slope_threshold
move_y[:, 1:num_cols] -= hf[:, : num_cols - 1] - hf[:, 1:num_cols] > slope_threshold
# move vertices along the corners
move_corners[: num_rows - 1, : num_cols - 1] += (
hf[1:num_rows, 1:num_cols] - hf[: num_rows - 1, : num_cols - 1] > slope_threshold
)
move_corners[1:num_rows, 1:num_cols] -= (
hf[: num_rows - 1, : num_cols - 1] - hf[1:num_rows, 1:num_cols] > slope_threshold
)
xx += (move_x + move_corners * (move_x == 0)) * horizontal_scale
yy += (move_y + move_corners * (move_y == 0)) * horizontal_scale
# create vertices for the mesh
vertices = np.zeros((num_rows * num_cols, 3), dtype=np.float32)
vertices[:, 0] = xx.flatten()
vertices[:, 1] = yy.flatten()
vertices[:, 2] = hf.flatten() * vertical_scale
# create triangles for the mesh
triangles = -np.ones((2 * (num_rows - 1) * (num_cols - 1), 3), dtype=np.uint32)
for i in range(num_rows - 1):
ind0 = np.arange(0, num_cols - 1) + i * num_cols
ind1 = ind0 + 1
ind2 = ind0 + num_cols
ind3 = ind2 + 1
start = 2 * i * (num_cols - 1)
stop = start + 2 * (num_cols - 1)
triangles[start:stop:2, 0] = ind0
triangles[start:stop:2, 1] = ind3
triangles[start:stop:2, 2] = ind1
triangles[start + 1 : stop : 2, 0] = ind0
triangles[start + 1 : stop : 2, 1] = ind2
triangles[start + 1 : stop : 2, 2] = ind3
return vertices, triangles
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
Configuration classes defining the different terrains available. Each configuration class must
inherit from ``omni.isaac.orbit.terrains.terrains_cfg.TerrainConfig`` and define the following attributes:
- ``name``: Name of the terrain. This is used for the prim name in the USD stage.
- ``function``: Function to generate the terrain. This function must take as input the terrain difficulty
and the configuration parameters and return a `tuple with the `trimesh`` mesh object and terrain origin.
"""
import numpy as np
import trimesh
from dataclasses import MISSING
from typing import Callable, Dict, List, Optional, Tuple
from omni.isaac.orbit.utils import configclass
@configclass
class SubTerrainBaseCfg:
"""Base class for terrain configurations."""
function: Callable[[float, "SubTerrainBaseCfg"], Tuple[List[trimesh.Trimesh], np.ndarray]] = MISSING
"""Function to generate the terrain.
This function must take as input the terrain difficulty and the configuration parameters and
return a tuple with a list of ``trimesh`` mesh objects and the terrain origin.
"""
proportion: float = 1.0
"""Proportion of the terrain to generate. Defaults to 1.0.
This is used to generate a mix of terrains. The proportion corresponds to the probability of sampling
the particular terrain. For example, if there are two terrains, A and B, with proportions 0.3 and 0.7,
respectively, then the probability of sampling terrain A is 0.3 and the probability of sampling terrain B
is 0.7.
"""
size: Tuple[float, float] = MISSING
"""The width (along x) and length (along y) of the terrain (in m)."""
@configclass
class TerrainGeneratorCfg:
"""Configuration for the terrain generator."""
size: Tuple[float, float] = MISSING
"""The width (along x) and length (along y) of each sub-terrain (in m).
Note:
This value is passed on to all the sub-terrain configurations.
"""
border_width: float = 0.0
"""The width of the border around the terrain (in m). Defaults to 0.0."""
num_rows: int = 1
"""Number of rows of sub-terrains to generate. Defaults to 1."""
num_cols: int = 1
"""Number of columns of sub-terrains to generate. Defaults to 1."""
horizontal_scale: float = 0.1
"""The discretization of the terrain along the x and y axes (in m). Defaults to 0.1.
This value is passed on to all the height field sub-terrain configurations.
"""
vertical_scale: float = 0.005
"""The discretization of the terrain along the z axis (in m). Defaults to 0.005.
This value is passed on to all the height field sub-terrain configurations.
"""
slope_threshold: Optional[float] = 0.75
"""The slope threshold above which surfaces are made vertical. Defaults to 0.75.
If :obj:`None` no correction is applied.
This value is passed on to all the height field sub-terrain configurations.
"""
sub_terrains: Dict[str, SubTerrainBaseCfg] = MISSING
"""List of sub-terrain configurations."""
difficulty_choices: List[float] = [0.5, 0.75, 0.9]
"""List of difficulty choices. Defaults to [0.5, 0.75, 0.9].
The difficulty choices are used to sample the difficulty of the generated terrain. The specified
choices are randomly sampled with equal probability.
Note:
This is used only when curriculum-based generation is disabled.
"""
use_cache: bool = False
"""Whether to load the terrain from cache if it exists. Defaults to True."""
cache_dir: str = "/tmp/orbit/terrains"
"""The directory where the terrain cache is stored. Defaults to "/tmp/orbit/terrains"."""
@configclass
class TerrainImporterCfg:
"""Configuration for the terrain manager."""
prim_path: str = MISSING
"""The absolute path of the USD terrain prim.
All sub-terrains are imported relative to this prim path.
"""
color: Optional[Tuple[float, float, float]] = (0.065, 0.0725, 0.080)
"""The color of the terrain. Defaults to (0.065, 0.0725, 0.080).
If :obj:`None`, no color is applied to the prim.
"""
static_friction: float = 1.0
"""The static friction coefficient of the terrain. Defaults to 1.0."""
dynamic_friction: float = 1.0
"""The dynamic friction coefficient of the terrain. Defaults to 1.0."""
restitution: float = 0.0
"""The restitution coefficient of the terrain. Defaults to 0.0."""
improve_patch_friction: bool = False
"""Whether to enable patch friction. Defaults to False."""
combine_mode: str = "average"
"""Determines the way physics materials will be combined during collisions. Defaults to `average`.
Available options are `average`, `min`, `multiply`, `multiply`, and `max`.
"""
env_spacing: float = 3.0
"""The spacing between environment origins when defined in a grid. Defaults to 3.0.
Note:
This parameter is used only when no sub-terrain origins are defined.
"""
max_init_terrain_level: Optional[int] = 5
"""The maximum initial terrain level for defining environment origins. Defaults to 5.
The terrain levels are specified by the number of rows in the grid arrangement of
sub-terrains. If :obj:`None`, then the initial terrain level is set to the maximum
terrain level available (``num_rows - 1``).
Note:
This parameter is used only when sub-terrain origins are defined.
"""
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This sub-module provides methods to create different terrains using the ``trimesh`` library.
In contrast to the height-field representation, the trimesh representation does not
create arbitrarily small triangles. Instead, the terrain is represented as a single
tri-mesh primitive. Thus, this representation is more computationally and memory
efficient than the height-field representation, but it is not as flexible.
"""
from .mesh_terrains_cfg import (
MeshBoxTerrainCfg,
MeshFloatingRingTerrainCfg,
MeshGapTerrainCfg,
MeshInvertedPyramidStairsTerrainCfg,
MeshPitTerrainCfg,
MeshPlaneTerrainCfg,
MeshPyramidStairsTerrainCfg,
MeshRailsTerrainCfg,
MeshRandomGridTerrainCfg,
MeshRepeatedBoxesTerrainCfg,
MeshRepeatedCylindersTerrainCfg,
MeshRepeatedPyramidsTerrainCfg,
MeshStarTerrainCfg,
)
__all__ = [
"MeshPlaneTerrainCfg",
"MeshPyramidStairsTerrainCfg",
"MeshInvertedPyramidStairsTerrainCfg",
"MeshRandomGridTerrainCfg",
"MeshRailsTerrainCfg",
"MeshPitTerrainCfg",
"MeshBoxTerrainCfg",
"MeshGapTerrainCfg",
"MeshFloatingRingTerrainCfg",
"MeshStarTerrainCfg",
"MeshRepeatedPyramidsTerrainCfg",
"MeshRepeatedBoxesTerrainCfg",
"MeshRepeatedCylindersTerrainCfg",
]
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import numpy as np
import scipy.spatial.transform as tf
import trimesh
from typing import List, Tuple
"""
Primitive functions to generate meshes.
"""
def make_plane(size: Tuple[float, float], height: float) -> trimesh.Trimesh:
"""Generate a plane mesh.
Args:
size (Tuple[float, float]): The length (along x) and width (along y) of the terrain (in m).
height (float): The height of the plane (in m).
Returns:
trimesh.Trimesh: A trimesh.Trimesh objects for the plane.
"""
# compute the vertices of the terrain
x0 = [size[0], size[1], height]
x1 = [size[0], 0.0, height]
x2 = [0.0, size[1], height]
x3 = [0.0, 0.0, height]
# generate the tri-mesh with two triangles
vertices = np.array([x0, x1, x2, x3])
faces = np.array([[1, 0, 2], [2, 3, 1]])
plane_mesh = trimesh.Trimesh(vertices=vertices, faces=faces)
plane_mesh.apply_translation(-np.array([size[0] / 2.0, size[1] / 2.0, 0.0]))
# return the tri-mesh and the position
return plane_mesh
def make_border(
size: Tuple[float, float], inner_size: Tuple[float, float], height: float, position: Tuple[float, float, float]
) -> List[trimesh.Trimesh]:
"""Generate meshes for a rectangular border with a hole in the middle.
.. code:: text
+---------------------+
|#####################|
|##+---------------+##|
|##| |##|
|##| |##| length
|##| |##| (y-axis)
|##| |##|
|##+---------------+##|
|#####################|
+---------------------+
width (x-axis)
Args:
size (Tuple[float, float]): The length (along x) and width (along y) of the terrain (in m).
inner_size (Tuple[float, float]): The inner length (along x) and width (along y) of the hole (in m).
height (float): The height of the border (in m).
position (Tuple[float, float, float]): The center of the border (in m).
Returns:
List[trimesh.Trimesh]: A list of trimesh.Trimesh objects that represent the border.
"""
# compute thickness of the border
thickness_x = (size[0] - inner_size[0]) / 2.0
thickness_y = (size[1] - inner_size[1]) / 2.0
# generate tri-meshes for the border
# top/bottom border
box_dims = (size[0], thickness_y, height)
# -- top
box_pos = (position[0], position[1] + inner_size[1] / 2.0 + thickness_y / 2.0, position[2])
box_mesh_top = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos))
# -- bottom
box_pos = (position[0], position[1] - inner_size[1] / 2.0 - thickness_y / 2.0, position[2])
box_mesh_bottom = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos))
# left/right border
box_dims = (thickness_x, inner_size[1], height)
# -- left
box_pos = (position[0] - inner_size[0] / 2.0 - thickness_x / 2.0, position[1], position[2])
box_mesh_left = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos))
# -- right
box_pos = (position[0] + inner_size[0] / 2.0 + thickness_x / 2.0, position[1], position[2])
box_mesh_right = trimesh.creation.box(box_dims, trimesh.transformations.translation_matrix(box_pos))
# return the tri-meshes
return [box_mesh_left, box_mesh_right, box_mesh_top, box_mesh_bottom]
def make_box(
length: float,
width: float,
height: float,
center: Tuple[float, float, float],
max_yx_angle: float = 0,
degrees: bool = True,
) -> trimesh.Trimesh:
"""Generate a box mesh with a random orientation.
Args:
length (float): The length (along x) of the box (in m).
width (float): The width (along y) of the box (in m).
height (float): The height of the cylinder (in m).
center (Tuple[float, float, float]): The center of the cylinder (in m).
max_yx_angle (float, optional): The maximum angle along the y and x axis. Defaults to 0.
degrees (bool, optional): Whether the angle is in degrees. Defaults to True.
Returns:
trimesh.Trimesh: A trimesh.Trimesh object for the cylinder.
"""
# create a pose for the cylinder
transform = np.eye(4)
transform[0:3, -1] = np.asarray(center)
# -- create a random rotation
euler_zyx = tf.Rotation.random().as_euler("zyx") # returns rotation of shape (3,)
# -- cap the rotation along the y and x axis
if degrees:
max_yx_angle = max_yx_angle / 180.0
euler_zyx[1:] *= max_yx_angle
# -- apply the rotation
transform[0:3, 0:3] = tf.Rotation.from_euler("zyx", euler_zyx).as_matrix()
# create the box
dims = (length, width, height)
return trimesh.creation.box(dims, transform=transform)
def make_cylinder(
radius: float, height: float, center: Tuple[float, float, float], max_yx_angle: float = 0, degrees: bool = True
) -> trimesh.Trimesh:
"""Generate a cylinder mesh with a random orientation.
Args:
radius (float): The radius of the cylinder (in m).
height (float): The height of the cylinder (in m).
center (Tuple[float, float, float]): The center of the cylinder (in m).
max_yx_angle (float, optional): The maximum angle along the y and x axis. Defaults to 0.
degrees (bool, optional): Whether the angle is in degrees. Defaults to True.
Returns:
trimesh.Trimesh: A trimesh.Trimesh object for the cylinder.
"""
# create a pose for the cylinder
transform = np.eye(4)
transform[0:3, -1] = np.asarray(center)
# -- create a random rotation
euler_zyx = tf.Rotation.random().as_euler("zyx") # returns rotation of shape (3,)
# -- cap the rotation along the y and x axis
if degrees:
max_yx_angle = max_yx_angle / 180.0
euler_zyx[1:] *= max_yx_angle
# -- apply the rotation
transform[0:3, 0:3] = tf.Rotation.from_euler("zyx", euler_zyx).as_matrix()
# create the cylinder
return trimesh.creation.cylinder(radius, height, sections=np.random.randint(4, 6), transform=transform)
def make_cone(
radius: float, height: float, center: Tuple[float, float, float], max_yx_angle: float = 0, degrees: bool = True
) -> trimesh.Trimesh:
"""Generate a cone mesh with a random orientation.
Args:
radius (float): The radius of the cone (in m).
height (float): The height of the cone (in m).
center (Tuple[float, float, float]): The center of the cone (in m).
max_yx_angle (float, optional): The maximum angle along the y and x axis. Defaults to 0.
degrees (bool, optional): Whether the angle is in degrees. Defaults to True.
Returns:
trimesh.Trimesh: A trimesh.Trimesh object for the cone.
"""
# create a pose for the cylinder
transform = np.eye(4)
transform[0:3, -1] = np.asarray(center)
# -- create a random rotation
euler_zyx = tf.Rotation.random().as_euler("zyx") # returns rotation of shape (3,)
# -- cap the rotation along the y and x axis
if degrees:
max_yx_angle = max_yx_angle / 180.0
euler_zyx[1:] *= max_yx_angle
# -- apply the rotation
transform[0:3, 0:3] = tf.Rotation.from_euler("zyx", euler_zyx).as_matrix()
# create the cone
return trimesh.creation.cone(radius, height, sections=np.random.randint(4, 6), transform=transform)
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import numpy as np
import trimesh
from typing import List
import warp as wp
def color_meshes_by_height(meshes: List[trimesh.Trimesh], **kwargs) -> trimesh.Trimesh:
"""
Color the vertices of a trimesh object based on the z-coordinate (height) of each vertex,
using the Turbo colormap. If the z-coordinates are all the same, the vertices will be colored
with a single color.
Args:
meshes (List[trimesh.Trimesh]): A list of trimesh objects.
Keyword Args:
color (List[int]): A list of 3 integers in the range [0,255] representing the RGB
color of the mesh. Used when the z-coordinates of all vertices are the same.
Returns:
trimesh.Trimesh: A trimesh object with the vertices colored based on the z-coordinate (height) of each vertex.
"""
# Combine all meshes into a single mesh
mesh = trimesh.util.concatenate(meshes)
# Get the z-coordinates of each vertex
heights = mesh.vertices[:, 2]
# Check if the z-coordinates are all the same
if np.max(heights) == np.min(heights):
# Obtain a single color: light blue
color = kwargs.pop("color", [172, 216, 230, 255])
color = np.asarray(color, dtype=np.uint8)
# Set the color for all vertices
mesh.visual.vertex_colors = color
else:
# Normalize the heights to [0,1]
heights_normalized = (heights - np.min(heights)) / (np.max(heights) - np.min(heights))
# Get the color for each vertex based on the height
colors = trimesh.visual.color.interpolate(heights_normalized, color_map="turbo")
# Set the vertex colors
mesh.visual.vertex_colors = colors
# Return the mesh
return mesh
def create_prim_from_mesh(prim_path: str, vertices: np.ndarray, triangles: np.ndarray, **kwargs):
"""Create a USD prim with mesh defined from vertices and triangles.
The function creates a USD prim with a mesh defined from vertices and triangles. It performs the
following steps:
- Create a USD Xform prim at the path :obj:`prim_path`.
- Create a USD prim with a mesh defined from the input vertices and triangles at the path :obj:`{prim_path}/mesh`.
- Assign a physics material to the mesh at the path :obj:`{prim_path}/physicsMaterial`.
- Assign a visual material to the mesh at the path :obj:`{prim_path}/visualMaterial`.
Args:
prim_path (str): The path to the primitive to be created.
vertices (np.ndarray): The vertices of the mesh. Shape is :math:`(N, 3)`, where :math:`N`
is the number of vertices.
triangles (np.ndarray): The triangles of the mesh as references to vertices for each triangle.
Shape is :math:`(M, 3)`, where :math:`M` is the number of triangles / faces.
Keyword Args:
translation (Optional[Sequence[float]]): The translation of the terrain. Defaults to None.
orientation (Optional[Sequence[float]]): The orientation of the terrain. Defaults to None.
scale (Optional[Sequence[float]]): The scale of the terrain. Defaults to None.
color (Optional[tuple]): The color of the terrain. Defaults to (0.065, 0.0725, 0.080).
static_friction (float): The static friction of the terrain. Defaults to 1.0.
dynamic_friction (float): The dynamic friction of the terrain. Defaults to 1.0.
restitution (float): The restitution of the terrain. Defaults to 0.0.
improve_patch_friction (bool): Whether to enable patch friction. Defaults to False.
combine_mode (str): Determines the way physics materials will be combined during collisions.
Available options are `average`, `min`, `multiply`, `multiply`, and `max`. Defaults to `average`.
"""
# need to import these here to prevent isaacsim launching when importing this module
import omni.isaac.core.utils.prims as prim_utils
from omni.isaac.core.materials import PhysicsMaterial, PreviewSurface
from omni.isaac.core.prims import GeometryPrim, XFormPrim
from pxr import PhysxSchema
# create parent prim
prim_utils.create_prim(prim_path, "Xform")
# create mesh prim
prim_utils.create_prim(
f"{prim_path}/mesh",
"Mesh",
translation=kwargs.get("translation"),
orientation=kwargs.get("orientation"),
scale=kwargs.get("scale"),
attributes={
"points": vertices,
"faceVertexIndices": triangles.flatten(),
"faceVertexCounts": np.asarray([3] * len(triangles)),
"subdivisionScheme": "bilinear",
},
)
# create visual material
color = kwargs.get("color", (0.065, 0.0725, 0.080))
if color is not None:
material = PreviewSurface(f"{prim_path}/visualMaterial", color=np.asarray(color))
XFormPrim(f"{prim_path}/mesh").apply_visual_material(material)
# create physics material
material = PhysicsMaterial(
f"{prim_path}/physicsMaterial",
static_friction=kwargs.get("static_friction", 1.0),
dynamic_friction=kwargs.get("dynamic_friction", 1.0),
restitution=kwargs.get("restitution", 0.0),
)
# apply PhysX Rigid Material schema
physx_material_api = PhysxSchema.PhysxMaterialAPI.Apply(material.prim)
# set patch friction property
improve_patch_friction = kwargs.get("improve_patch_friction", False)
physx_material_api.CreateImprovePatchFrictionAttr().Set(improve_patch_friction)
# set combination mode for coefficients
combine_mode = kwargs.get("combine_mode", "average")
physx_material_api.CreateFrictionCombineModeAttr().Set(combine_mode)
physx_material_api.CreateRestitutionCombineModeAttr().Set(combine_mode)
# apply physics material to ground plane
GeometryPrim(f"{prim_path}/mesh", collision=True).apply_physics_material(material)
def convert_to_warp_mesh(vertices: np.ndarray, triangles: np.ndarray, device: str) -> wp.Mesh:
"""Create a warp mesh object with a mesh defined from vertices and triangles.
Args:
vertices (np.ndarray): The vertices of the mesh. Shape is :math:`(N, 3)`, where :math:`N`
is the number of vertices.
triangles (np.ndarray): The triangles of the mesh as references to vertices for each triangle.
Shape is :math:`(M, 3)`, where :math:`M` is the number of triangles / faces.
device (str): The device to use for the mesh.
Returns:
wp.Mesh: The warp mesh object.
"""
# create warp mesh
return wp.Mesh(
points=wp.array(vertices.astype(np.float32), dtype=wp.vec3, device=device),
indices=wp.array(triangles.astype(np.int32).flatten(), dtype=int, device=device),
)
...@@ -7,13 +7,22 @@ ...@@ -7,13 +7,22 @@
import collections.abc import collections.abc
import hashlib
import importlib import importlib
import inspect import inspect
import json
from typing import Any, Callable, Dict, Iterable, Mapping from typing import Any, Callable, Dict, Iterable, Mapping
from .array import TENSOR_TYPE_CONVERSIONS, TENSOR_TYPES from .array import TENSOR_TYPE_CONVERSIONS, TENSOR_TYPES
__all__ = ["class_to_dict", "update_class_from_dict", "convert_dict_to_backend", "update_dict", "print_dict"] __all__ = [
"class_to_dict",
"update_class_from_dict",
"dict_to_md5_hash",
"convert_dict_to_backend",
"update_dict",
"print_dict",
]
""" """
Dictionary <-> Class operations. Dictionary <-> Class operations.
...@@ -115,6 +124,32 @@ def update_class_from_dict(obj, data: Dict[str, Any], _ns: str = "") -> None: ...@@ -115,6 +124,32 @@ def update_class_from_dict(obj, data: Dict[str, Any], _ns: str = "") -> None:
raise KeyError(f"[Config]: Key not found under namespace: {key_ns}.") raise KeyError(f"[Config]: Key not found under namespace: {key_ns}.")
"""
Dictionary <-> Hashable operations.
"""
def dict_to_md5_hash(data: object) -> str:
"""Convert a dictionary into a hashable key using MD5 hash.
Args:
data (object): Input dictionary or configuration object to convert.
Returns:
str: A string object of double length containing only hexadecimal digits.
"""
# convert to dictionary
if isinstance(data, dict):
encoded_buffer = json.dumps(data, sort_keys=True).encode()
else:
encoded_buffer = json.dumps(class_to_dict(data), sort_keys=True).encode()
# compute hash using MD5
data_hash = hashlib.md5()
data_hash.update(encoded_buffer)
# return the hash key
return data_hash.hexdigest()
""" """
Dictionary operations. Dictionary operations.
""" """
......
...@@ -23,6 +23,9 @@ INSTALL_REQUIRES = [ ...@@ -23,6 +23,9 @@ INSTALL_REQUIRES = [
"prettytable==3.3.0", "prettytable==3.3.0",
# devices # devices
"hidapi", "hidapi",
# procedural-generation
"trimesh",
"pyglet==1.5.27", # pyglet 2.0 requires python 3.8
] ]
# Installation operation # Installation operation
......
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This script demonstrates how to use the cloner API from Isaac Sim.
Reference: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_gym_cloner.html
"""
"""Launch Isaac Sim Simulator first."""
import argparse
from omni.isaac.kit import SimulationApp
# add argparse arguments
parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!")
parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.")
parser.add_argument("--num_robots", type=int, default=128, help="Number of robots to spawn.")
parser.add_argument(
"--asset", type=str, default="orbit", help="The asset source location for the robot. Can be: orbit, oige."
)
args_cli = parser.parse_args()
# launch omniverse app
config = {"headless": args_cli.headless}
simulation_app = SimulationApp(config)
"""Rest everything follows."""
import torch
import carb
import omni.isaac.core.utils.nucleus as nucleus_utils
import omni.isaac.core.utils.prims as prim_utils
from omni.isaac.cloner import GridCloner
from omni.isaac.core.articulations import ArticulationView
from omni.isaac.core.utils.carb import set_carb_setting
from omni.isaac.core.utils.viewports import set_camera_view
from omni.isaac.core.world import World
# check nucleus connection
if nucleus_utils.get_assets_root_path() is None:
msg = (
"Unable to perform Nucleus login on Omniverse. Assets root path is not set.\n"
"\tPlease check: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/overview.html#omniverse-nucleus"
)
carb.log_error(msg)
raise RuntimeError(msg)
ISAAC_NUCLEUS_DIR = f"{nucleus_utils.get_assets_root_path()}/Isaac"
"""Path to the `Isaac` directory on the NVIDIA Nucleus Server."""
ISAAC_ORBIT_NUCLEUS_DIR = f"{nucleus_utils.get_assets_root_path()}/Isaac/Samples/Orbit"
"""Path to the `Isaac/Samples/Orbit` directory on the NVIDIA Nucleus Server."""
"""
Main
"""
def main():
"""Spawns the ANYmal robot and clones it using Isaac Sim Cloner API."""
# Load kit helper
world = World(physics_dt=0.005, rendering_dt=0.005, backend="torch", device="cuda:0")
# Set main camera
set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0])
# Enable flatcache which avoids passing data over to USD structure
# this speeds up the read-write operation of GPU buffers
if world.get_physics_context().use_gpu_pipeline:
world.get_physics_context().enable_flatcache(True)
# Enable hydra scene-graph instancing
# this is needed to visualize the scene when flatcache is enabled
set_carb_setting(world._settings, "/persistent/omnihydra/useSceneGraphInstancing", True)
# Create interface to clone the scene
cloner = GridCloner(spacing=2.0)
cloner.define_base_env("/World/envs")
# Everything under the namespace "/World/envs/env_0" will be cloned
prim_utils.define_prim("/World/envs/env_0")
# Spawn things into stage
# Ground-plane
world.scene.add_default_ground_plane(prim_path="/World/defaultGroundPlane", z_position=0.0)
# Lights-1
prim_utils.create_prim(
"/World/Light/GreySphere",
"SphereLight",
translation=(4.5, 3.5, 10.0),
attributes={"radius": 2.5, "intensity": 600.0, "color": (0.75, 0.75, 0.75)},
)
# Lights-2
prim_utils.create_prim(
"/World/Light/WhiteSphere",
"SphereLight",
translation=(-4.5, 3.5, 10.0),
attributes={"radius": 2.5, "intensity": 600.0, "color": (1.0, 1.0, 1.0)},
)
# -- Robot
# resolve asset
if args_cli.asset == "orbit":
usd_path = f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/ANYbotics/ANYmalC/anymal_c_minimal_instanceable.usd"
elif args_cli.asset == "oige":
usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_instanceable.usd"
else:
raise ValueError(f"Invalid asset: {args_cli.asset}. Must be one of: orbit, oige.")
# add asset
print("Loading robot from: ", usd_path)
prim_utils.create_prim(
"/World/envs/env_0/Robot",
usd_path=usd_path,
translation=(0.0, 0.0, 0.6),
)
# Clone the scene
num_envs = args_cli.num_robots
cloner.define_base_env("/World/envs")
envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs)
envs_positions = cloner.clone(
source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True
)
# convert environment positions to torch tensor
envs_positions = torch.tensor(envs_positions, dtype=torch.float, device=world.device)
# filter collisions within each environment instance
physics_scene_path = world.get_physics_context().prim_path
cloner.filter_collisions(
physics_scene_path, "/World/collisions", envs_prim_paths, global_paths=["/World/defaultGroundPlane"]
)
# Setup robot
robot_view = ArticulationView("/World/envs/env_.*/Robot", name="ANYMAL")
world.scene.add(robot_view)
# Play the simulator
world.reset()
# Now we are ready!
print("[INFO]: Setup complete...")
# dummy actions
# actions = torch.zeros(robot.count, robot.num_actions, device=robot.device)
# Define simulation stepping
sim_dt = world.get_physics_dt()
# episode counter
sim_time = 0.0
# Simulate physics
while simulation_app.is_running():
# If simulation is stopped, then exit.
if world.is_stopped():
break
# If simulation is paused, then skip.
if not world.is_playing():
world.step(render=not args_cli.headless)
continue
# perform step
world.step()
# update sim-time
sim_time += sim_dt
if __name__ == "__main__":
# Run the main function
main()
# Close the simulator
simulation_app.close()
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This script shows how to use replicator to randomly change the textures of a USD scene.
Note:
Currently this script fails since cloner does not support changing textures of cloned
USD prims. This is because the prims are cloned using `Sdf.ChangeBlock` which does not
allow individual texture changes.
"""
"""Launch Isaac Sim Simulator first."""
import argparse
# omni-isaac-orbit
from omni.isaac.kit import SimulationApp
# add argparse arguments
parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!")
parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.")
args_cli = parser.parse_args()
# launch omniverse app
config = {"headless": args_cli.headless}
simulation_app = SimulationApp(config)
"""Rest everything follows."""
import numpy as np
import torch
import omni.isaac.core.utils.prims as prim_utils
import omni.replicator.core as rep
from omni.isaac.cloner import GridCloner
from omni.isaac.core.objects import DynamicSphere
from omni.isaac.core.prims import RigidPrimView
from omni.isaac.core.simulation_context import SimulationContext
from omni.isaac.core.utils.viewports import set_camera_view
def main():
"""Spawn a bunch of balls and randomly change their textures."""
# Load kit helper
sim_params = {
"use_gpu": True,
"use_gpu_pipeline": True,
"use_flatcache": True,
"enable_scene_query_support": True,
}
sim = SimulationContext(
physics_dt=1.0 / 60.0, rendering_dt=1.0 / 60.0, sim_params=sim_params, backend="torch", device="cuda:0"
)
# Set main camera
set_camera_view([0.0, 30.0, 25.0], [0.0, 0.0, -2.5])
# Parameters
num_balls = 2048
# Create interface to clone the scene
cloner = GridCloner(spacing=2.0)
cloner.define_base_env("/World/envs")
# Everything under the namespace "/World/envs/env_0" will be cloned
prim_utils.define_prim("/World/envs/env_0")
# Define the scene
# -- Light
prim_utils.create_prim(
"/World/sphereLight",
"SphereLight",
translation=(0.0, 0.0, 500.0),
attributes={"radius": 100.0, "intensity": 50000.0, "color": (0.75, 0.75, 0.75)},
)
# -- Ball
DynamicSphere(prim_path="/World/envs/env_0/ball", translation=np.array([0.0, 0.0, 5.0]), mass=0.5, radius=0.25)
# Clone the scene
cloner.define_base_env("/World/envs")
envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_balls)
env_positions = cloner.clone(
source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True
)
physics_scene_path = sim.get_physics_context().prim_path
cloner.filter_collisions(
physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"]
)
# Use replicator to randomize color on the spheres
with rep.new_layer():
# Define a function to get all the shapes
def get_shapes():
shapes = rep.get.prims(path_pattern="/World/envs/env_.*/ball")
with shapes:
rep.randomizer.color(colors=rep.distribution.uniform((0, 0, 0), (1, 1, 1)))
return shapes.node
# Register the function
rep.randomizer.register(get_shapes)
# Specify the frequency of randomization
with rep.trigger.on_frame():
rep.randomizer.get_shapes()
# Set ball positions over terrain origins
# Create a view over all the balls
ball_view = RigidPrimView("/World/envs/env_.*/ball", reset_xform_properties=False)
# cache initial state of the balls
ball_initial_positions = torch.tensor(env_positions, dtype=torch.float, device=sim.device)
ball_initial_positions[:, 2] += 5.0
# set initial poses
# note: setting here writes to USD :)
ball_view.set_world_poses(positions=ball_initial_positions)
# Play simulator
sim.reset()
# Step replicator to randomize colors
rep.orchestrator.step(pause_timeline=False)
# Stop replicator to prevent further randomization
rep.orchestrator.stop()
# Pause simulator at the beginning for inspection
sim.pause()
# Initialize the ball views for physics simulation
ball_view.initialize()
ball_initial_velocities = ball_view.get_velocities()
# Create a counter for resetting the scene
step_count = 0
# Simulate physics
while simulation_app.is_running():
# If simulation is stopped, then exit.
if sim.is_stopped():
break
# If simulation is paused, then skip.
if not sim.is_playing():
sim.step(render=not args_cli.headless)
continue
# Reset the scene
if step_count % 500 == 0:
# reset the balls
ball_view.set_world_poses(positions=ball_initial_positions)
ball_view.set_velocities(ball_initial_velocities)
# reset the counter
step_count = 0
# Step simulation
sim.step()
# Update counter
step_count += 1
if __name__ == "__main__":
# Runs the main function
main()
# Close the simulator
simulation_app.close()
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import argparse
import matplotlib.pyplot as plt
import numpy as np
import os
import trimesh
import omni.isaac.orbit.terrains.height_field as hf_gen
from omni.isaac.orbit.terrains.utils import color_meshes_by_height
def test_random_uniform_terrain(difficulty: float):
# parameters for the terrain
cfg = hf_gen.HfRandomUniformTerrainCfg(
size=(8.0, 8.0),
horizontal_scale=0.1,
vertical_scale=0.005,
border_width=0.0,
noise_range=(-0.05, 0.05),
noise_step=0.005,
downsampled_scale=0.2,
)
# generate terrain
meshes, origin = cfg.function(difficulty=difficulty, cfg=cfg)
# add colors to the meshes based on the height
colored_mesh = color_meshes_by_height(meshes)
# add a marker for the origin
origin_transform = trimesh.transformations.translation_matrix(origin)
origin_marker = trimesh.creation.axis(origin_size=0.1, transform=origin_transform)
# visualize the meshes
scene = trimesh.Scene([colored_mesh, origin_marker])
# save the scene to a png file
data = scene.save_image(resolution=(640, 480))
# write the image to a file
with open(os.path.join(output_dir, "random_uniform_terrain.jpg"), "wb") as f:
f.write(data)
# show the scene in a window
if not headless:
trimesh.viewer.SceneViewer(scene=scene, caption="Random Uniform Terrain")
def test_pyramid_sloped_terrain(difficulty: float, inverted: bool):
# parameters for the terrain
cfg = hf_gen.HfPyramidSlopedTerrainCfg(
size=(8.0, 8.0),
horizontal_scale=0.1,
vertical_scale=0.005,
border_width=0.0,
slope_range=(0.0, 0.4),
platform_width=1.5,
inverted=inverted,
)
# generate terrain
meshes, origin = cfg.function(difficulty=difficulty, cfg=cfg)
# add colors to the meshes based on the height
colored_mesh = color_meshes_by_height(meshes)
# add a marker for the origin
origin_transform = trimesh.transformations.translation_matrix(origin)
origin_marker = trimesh.creation.axis(origin_size=0.1, transform=origin_transform)
# visualize the meshes
scene = trimesh.Scene([colored_mesh, origin_marker])
# save the scene to a png file
data = scene.save_image(resolution=(640, 480))
# resolve file name
if inverted:
caption = "Inverted Pyramid Sloped Terrain"
filename = "inverted_pyramid_sloped_terrain.jpg"
else:
caption = "Pyramid Sloped Terrain"
filename = "pyramid_sloped_terrain.jpg"
# write the image to a file
with open(os.path.join(output_dir, filename), "wb") as f:
f.write(data)
# show the scene in a window
if not headless:
trimesh.viewer.SceneViewer(scene=scene, caption=caption)
def test_pyramid_stairs_terrain(difficulty: float, inverted: bool):
# parameters for the terrain
cfg = hf_gen.HfPyramidStairsTerrainCfg(
size=(8.0, 8.0),
horizontal_scale=0.1,
vertical_scale=0.005,
border_width=0.0,
platform_width=1.5,
step_width=0.301,
step_height_range=(0.05, 0.23),
inverted=inverted,
)
# generate terrain
meshes, origin = cfg.function(difficulty=difficulty, cfg=cfg)
# add colors to the meshes based on the height
colored_mesh = color_meshes_by_height(meshes)
# add a marker for the origin
origin_transform = trimesh.transformations.translation_matrix(origin)
origin_marker = trimesh.creation.axis(origin_size=0.1, transform=origin_transform)
# visualize the meshes
scene = trimesh.Scene([colored_mesh, origin_marker])
# save the scene to a png file
data = scene.save_image(resolution=(640, 480))
# resolve file name
if inverted:
caption = "Inverted Pyramid Stairs Terrain"
filename = "inverted_pyramid_stairs_terrain.jpg"
else:
caption = "Pyramid Stairs Terrain"
filename = "pyramid_stairs_terrain.jpg"
# write the image to a file
with open(os.path.join(output_dir, filename), "wb") as f:
f.write(data)
# show the scene in a window
if not headless:
trimesh.viewer.SceneViewer(scene=scene, caption=caption)
def test_discrete_obstacles_terrain(difficulty: float, obstacle_height_mode: str):
# parameters for the terrain
cfg = hf_gen.HfDiscreteObstaclesTerrainCfg(
size=(8.0, 8.0),
horizontal_scale=0.1,
vertical_scale=0.005,
border_width=0.0,
num_obstacles=50,
obstacle_height_mode=obstacle_height_mode,
obstacle_width_range=(0.25, 0.75),
obstacle_height_range=(1.0, 2.0),
platform_width=1.5,
)
# generate terrain
meshes, origin = cfg.function(difficulty=difficulty, cfg=cfg)
# add colors to the meshes based on the height
colored_mesh = color_meshes_by_height(meshes)
# add a marker for the origin
origin_transform = trimesh.transformations.translation_matrix(origin)
origin_marker = trimesh.creation.axis(origin_size=0.1, transform=origin_transform)
# visualize the meshes
scene = trimesh.Scene([colored_mesh, origin_marker])
# save the scene to a png file
data = scene.save_image(resolution=(640, 480))
# resolve file name
if obstacle_height_mode == "choice":
caption = "Discrete Obstacles Terrain (Sampled Height)"
filename = "discrete_obstacles_terrain_choice.jpg"
elif obstacle_height_mode == "fixed":
caption = "Discrete Obstacles Terrain (Fixed Height)"
filename = "discrete_obstacles_terrain_fixed.jpg"
else:
raise ValueError(f"Unknown obstacle height mode: {obstacle_height_mode}")
# write the image to a file
with open(os.path.join(output_dir, filename), "wb") as f:
f.write(data)
# show the scene in a window
if not headless:
trimesh.viewer.SceneViewer(scene=scene, caption=caption)
def test_wave_terrain(difficulty: float):
# parameters for the terrain
cfg = hf_gen.HfWaveTerrainCfg(
size=(8.0, 8.0),
horizontal_scale=0.1,
vertical_scale=0.005,
border_width=0.0,
num_waves=5,
amplitude_range=(0.5, 1.0),
)
# generate terrain
meshes, origin = cfg.function(difficulty=difficulty, cfg=cfg)
# add colors to the meshes based on the height
colored_mesh = color_meshes_by_height(meshes)
# add a marker for the origin
origin_transform = trimesh.transformations.translation_matrix(origin)
origin_marker = trimesh.creation.axis(origin_size=0.1, transform=origin_transform)
# visualize the meshes
scene = trimesh.Scene([colored_mesh, origin_marker])
# save the scene to a png file
data = scene.save_image(resolution=(640, 480))
# write the image to a file
with open(os.path.join(output_dir, "wave_terrain.jpg"), "wb") as f:
f.write(data)
# show the scene in a window
if not headless:
trimesh.viewer.SceneViewer(scene=scene, caption="Wave Terrain")
def test_stepping_stones_terrain(difficulty: float):
# parameters for the terrain
cfg = hf_gen.HfSteppingStonesTerrainCfg(
size=(8.0, 8.0),
horizontal_scale=0.1,
vertical_scale=0.005,
platform_width=1.5,
border_width=0.0,
stone_width_range=(0.25, 1.575),
stone_height_max=0.2,
stone_distance_range=(0.05, 0.1),
holes_depth=-2.0,
)
# generate terrain
meshes, origin = cfg.function(difficulty=difficulty, cfg=cfg)
# add colors to the meshes based on the height
colored_mesh = color_meshes_by_height(meshes)
# add a marker for the origin
origin_transform = trimesh.transformations.translation_matrix(origin)
origin_marker = trimesh.creation.axis(origin_size=0.1, transform=origin_transform)
# visualize the meshes
scene = trimesh.Scene([colored_mesh, origin_marker])
# save the scene to a png file
data = scene.save_image(resolution=(640, 480))
# write the image to a file
with open(os.path.join(output_dir, "stepping_stones_terrain.jpg"), "wb") as f:
f.write(data)
# show the scene in a window
if not headless:
trimesh.viewer.SceneViewer(scene=scene, caption="Stepping Stones Terrain")
if __name__ == "__main__":
# Create argparse for headless mode
parser = argparse.ArgumentParser(description="Generate terrains using trimesh")
parser.add_argument("--headless", action="store_true", default=False, help="Run in headless mode")
args = parser.parse_args()
# Read headless mode
headless = args.headless
# Create directory to dump results
test_dir = os.path.dirname(os.path.abspath(__file__))
output_dir = os.path.join(test_dir, "output", "terrains", "height_field")
if not os.path.exists(output_dir):
os.makedirs(output_dir, exist_ok=True)
# generate terrains
test_random_uniform_terrain(difficulty=0.25)
test_pyramid_sloped_terrain(difficulty=0.25, inverted=False)
test_pyramid_sloped_terrain(difficulty=0.25, inverted=True)
test_pyramid_stairs_terrain(difficulty=0.25, inverted=False)
test_pyramid_stairs_terrain(difficulty=0.25, inverted=True)
test_discrete_obstacles_terrain(difficulty=0.25, obstacle_height_mode="choice")
test_discrete_obstacles_terrain(difficulty=0.25, obstacle_height_mode="fixed")
test_wave_terrain(difficulty=0.25)
test_stepping_stones_terrain(difficulty=1.0)
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import argparse
import os
import shutil
from omni.isaac.orbit.terrains.config.rough import ASSORTED_TERRAINS_CFG
from omni.isaac.orbit.terrains.terrain_generator import TerrainGenerator
if __name__ == "__main__":
# Create directory to dump results
test_dir = os.path.dirname(os.path.abspath(__file__))
output_dir = os.path.join(test_dir, "output", "generator")
# remove directory
if os.path.exists(output_dir):
shutil.rmtree(output_dir)
# create directory
os.makedirs(output_dir, exist_ok=True)
# modify the config to cache
ASSORTED_TERRAINS_CFG.use_cache = True
ASSORTED_TERRAINS_CFG.cache_dir = output_dir
# generate terrains
terrain_generator = TerrainGenerator(cfg=ASSORTED_TERRAINS_CFG, curriculum=False)
# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""
This script shows how to use the terrain generator from the Orbit framework.
The terrains are generated using the :class:`TerrainGenerator` class and imported using the :class:`TerrainImporter`
class. The terrains can be imported from a file or generated procedurally.
Example usage:
.. code-block:: bash
# generate terrain
# -- use physics sphere mesh
./orbit.sh -p source/extensions/omni.isaac.orbit/test/terrains/test_terrain_importer.py --terrain_type generated
# -- usd usd sphere geom
./orbit.sh -p source/extensions/omni.isaac.orbit/test/terrains/test_terrain_importer.py --terrain_type generated --geom_sphere
# usd terrain
./orbit.sh -p source/extensions/omni.isaac.orbit/test/terrains/test_terrain_importer.py --terrain_type usd
# plane terrain
./orbit.sh -p source/extensions/omni.isaac.orbit/test/terrains/test_terrain_importer.py --terrain_type plane
"""
"""Launch Isaac Sim Simulator first."""
import argparse
# omni-isaac-orbit
from omni.isaac.kit import SimulationApp
# add argparse arguments
parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!")
parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.")
parser.add_argument("--geom_sphere", action="store_true", default=False, help="Whether to use sphere mesh or shape.")
parser.add_argument(
"--terrain_type",
type=str,
default="generated",
help="Type of terrain to import. Can be 'generated' or 'usd' or 'plane'.",
)
args_cli = parser.parse_args()
# launch omniverse app
config = {"headless": args_cli.headless}
simulation_app = SimulationApp(config)
"""Rest everything follows."""
import numpy as np
import omni.isaac.core.utils.prims as prim_utils
import omni.kit.commands
from omni.isaac.cloner import GridCloner
from omni.isaac.core.materials import PhysicsMaterial, PreviewSurface
from omni.isaac.core.objects import DynamicSphere
from omni.isaac.core.prims import GeometryPrim, RigidPrim, RigidPrimView
from omni.isaac.core.simulation_context import SimulationContext
from omni.isaac.core.utils.viewports import set_camera_view
import omni.isaac.orbit.terrains as terrain_gen
from omni.isaac.orbit.terrains.config.rough import ASSORTED_TERRAINS_CFG
from omni.isaac.orbit.terrains.terrain_importer import TerrainImporter
from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
def main(terrain_type: str):
"""Generates a terrain from orbit."""
# Load kit helper
sim_params = {
"use_gpu": True,
"use_gpu_pipeline": True,
"use_flatcache": True,
"enable_scene_query_support": True,
}
sim = SimulationContext(
physics_dt=1.0 / 60.0, rendering_dt=1.0 / 60.0, sim_params=sim_params, backend="torch", device="cuda:0"
)
# Set main camera
set_camera_view([0.0, 30.0, 25.0], [0.0, 0.0, -2.5])
# Parameters
num_balls = 2048
# Create interface to clone the scene
cloner = GridCloner(spacing=2.0)
cloner.define_base_env("/World/envs")
# Everything under the namespace "/World/envs/env_0" will be cloned
prim_utils.define_prim("/World/envs/env_0")
# Handler for terrains importing
terrain_importer_cfg = terrain_gen.TerrainImporterCfg(prim_path="/World/ground", max_init_terrain_level=None)
terrain_importer = TerrainImporter(terrain_importer_cfg, device=sim.device)
# Ground-plane
if terrain_type == "generated":
terrain_generator = terrain_gen.TerrainGenerator(cfg=ASSORTED_TERRAINS_CFG, curriculum=True)
terrain_importer.import_mesh(terrain_generator.terrain_mesh, key="rough")
terrain_importer.configure_env_origins(num_balls, terrain_generator.terrain_origins)
elif terrain_type == "usd":
terrain_importer.import_usd(f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/rough_plane.usd", key="usd")
terrain_importer.configure_env_origins(num_balls)
elif terrain_type == "plane":
terrain_importer.import_ground_plane(key="flat")
terrain_importer.configure_env_origins(num_balls)
else:
raise ValueError(f"Unknown terrain type: {terrain_type}. Valid options are: generated, usd, plane.")
# Define the scene
# -- Light
prim_utils.create_prim(
"/World/sphereLight",
"SphereLight",
translation=(0.0, 0.0, 500.0),
attributes={"radius": 100.0, "intensity": 50000.0, "color": (0.75, 0.75, 0.75)},
)
# -- Ball
if args_cli.geom_sphere:
# -- Ball physics
sphere = DynamicSphere(
prim_path="/World/envs/env_0/ball", translation=np.array([0.0, 0.0, 5.0]), mass=0.5, radius=0.25
)
else:
# -- Ball geometry
cube_prim_path = omni.kit.commands.execute("CreateMeshPrimCommand", prim_type="Sphere")[1]
prim_utils.move_prim(cube_prim_path, "/World/envs/env_0/ball")
# -- Ball physics
RigidPrim(prim_path="/World/envs/env_0/ball", mass=0.5, scale=(0.5, 0.5, 0.5), translation=(0.0, 0.0, 0.5))
GeometryPrim(prim_path="/World/envs/env_0/ball", collision=True)
# -- Ball material
sphere_geom = GeometryPrim(prim_path="/World/envs/env_0/ball", collision=True)
visual_material = PreviewSurface(prim_path="/World/Looks/ballColorMaterial", color=np.asarray([0.0, 0.0, 1.0]))
physics_material = PhysicsMaterial(
prim_path="/World/Looks/ballPhysicsMaterial",
dynamic_friction=1.0,
static_friction=0.2,
restitution=0.0,
)
sphere_geom.set_collision_approximation("convexHull")
sphere_geom.apply_visual_material(visual_material)
sphere_geom.apply_physics_material(physics_material)
# Clone the scene
cloner.define_base_env("/World/envs")
envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_balls)
cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True)
physics_scene_path = sim.get_physics_context().prim_path
cloner.filter_collisions(
physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"]
)
# Set ball positions over terrain origins
# Create a view over all the balls
ball_view = RigidPrimView("/World/envs/env_.*/ball", reset_xform_properties=False)
# cache initial state of the balls
ball_initial_positions = terrain_importer.env_origins
ball_initial_positions[:, 2] += 5.0
# set initial poses
# note: setting here writes to USD :)
ball_view.set_world_poses(positions=ball_initial_positions)
# Play simulator
sim.reset()
# Initialize the ball views for physics simulation
ball_view.initialize()
ball_initial_velocities = ball_view.get_velocities()
# Create a counter for resetting the scene
step_count = 0
# Simulate physics
while simulation_app.is_running():
# If simulation is stopped, then exit.
if sim.is_stopped():
break
# If simulation is paused, then skip.
if not sim.is_playing():
sim.step(render=not args_cli.headless)
continue
# Reset the scene
if step_count % 500 == 0:
# reset the balls
ball_view.set_world_poses(positions=ball_initial_positions)
ball_view.set_velocities(ball_initial_velocities)
# reset the counter
step_count = 0
# Step simulation
sim.step()
# Update counter
step_count += 1
if __name__ == "__main__":
# Runs the main function
main(terrain_type=args_cli.terrain_type)
# Close the simulator
simulation_app.close()
...@@ -20,9 +20,8 @@ Or, equivalently, by directly calling the skrl library API as follows: ...@@ -20,9 +20,8 @@ Or, equivalently, by directly calling the skrl library API as follows:
import copy import copy
import torch import torch
from typing import List, Optional, Union
import tqdm import tqdm
from typing import List, Optional, Union
# skrl # skrl
from skrl.agents.torch import Agent from skrl.agents.torch import Agent
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment