diff --git a/docs/index.rst b/docs/index.rst index 2a9ddd0cdecf..649c36e1562c 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -148,6 +148,7 @@ Table of Contents source/experimental-features/bleeding-edge source/experimental-features/visuo_tactile_sensor source/experimental-features/rlinf_vla_posttraining + source/experimental-features/trail .. toctree:: :maxdepth: 1 diff --git a/docs/source/_static/experimental-features/trail/concept.jpeg b/docs/source/_static/experimental-features/trail/concept.jpeg new file mode 100644 index 000000000000..d5853bccd3b6 Binary files /dev/null and b/docs/source/_static/experimental-features/trail/concept.jpeg differ diff --git a/docs/source/_static/experimental-features/trail/curriculum.jpeg b/docs/source/_static/experimental-features/trail/curriculum.jpeg new file mode 100644 index 000000000000..a84faa0c8181 Binary files /dev/null and b/docs/source/_static/experimental-features/trail/curriculum.jpeg differ diff --git a/docs/source/_static/experimental-features/trail/presets.jpeg b/docs/source/_static/experimental-features/trail/presets.jpeg new file mode 100644 index 000000000000..8efec5c8488b Binary files /dev/null and b/docs/source/_static/experimental-features/trail/presets.jpeg differ diff --git a/docs/source/experimental-features/bleeding-edge.rst b/docs/source/experimental-features/bleeding-edge.rst index 48e0937f74a1..eb27aa45ca58 100644 --- a/docs/source/experimental-features/bleeding-edge.rst +++ b/docs/source/experimental-features/bleeding-edge.rst @@ -66,6 +66,9 @@ The following features are currently available in ``isaaclab_contrib``: - Extended deformable object support using the Newton physics backend with Vertex Block Descent (VBD), including proxy coupling between MJWarp and VBD. - API reference: :mod:`~isaaclab_contrib.deformable`, :mod:`~isaaclab_contrib.coupling` + * - **Trail Terrains** + - Library for colored trail terrains compatible with the terrain gerator. + - :doc:`trail` Contributing ------------ diff --git a/docs/source/experimental-features/trail.rst b/docs/source/experimental-features/trail.rst new file mode 100644 index 000000000000..ef7a3562a8c7 --- /dev/null +++ b/docs/source/experimental-features/trail.rst @@ -0,0 +1,284 @@ +.. _experimental_features_trail: + +.. currentmodule:: isaaclab_contrib + +Trail Library +============= + +The Trail library is a modular framework for generating parameterizable mountain-bike trails. +Each terrain patch comprises four components: a starting platform, a final platform, a connecting +trail segment, and distinct skill elements. The resulting terrain is represented as a single mesh +using the trimesh library, which models geometry as triangles and stores visual information within +the RGB channels. + +A trail segment is generated by first defining a cross-section polygon with counter-clockwise +ordered vertices. This polygon is then swept along a sweeping path, a sequence of knot-points +``(x, y, z, yaw)`` connecting the two platforms. To form a simple ground floor around the trail, +the vertices on the left and right borders are expanded outward. Each trail segment may contain one +or more skill elements of a unique type, such as ramps, skinnies, roots, rocks, or waves. Each +element is parameterized by a uniform distribution over its dimensions. Semantic information is +explicitly stored in the vertex colors. The exact RGB values can be chosen arbitrarily, +provided the mapping from RGB to semantic label remains bijective. + +.. figure:: ../_static/experimental-features/trail/concept.jpeg + :align: center + :figwidth: 95% + :alt: Conceptual pipeline for trail terrain generation + + Figure 1: The procedural generation of lightweight mountain-bike meshes is divided into four + stages. First, a cross-section polygon is swept along a 4D sweeping path. Second, two geometric + platforms are welded to the start and end of the trail. Third, the lateral vertices are extended + to align with the terrain patch size. Forest objects can optionally be placed. Finally, semantic + information is embedded into the vertex colors. + +The generated terrains can easily be embedded into the existing Terrain Generator. The +parameterization of the skill elements scales linearly (other maps may be defined) with the +difficulty level. The generated terrains can be embedded into terrain generation workflows and +support curriculum scaling through a ``difficulty`` value in ``[0, 1]``. + +The terrain mesh is systematically randomized by overlaying the vertices with a sequence of +geometric disturbances. Additionally, decorative elements such as trees, roots, or stones can be +procedurally placed adjacent to the trail. These perturbations break predictable structural +patterns, preventing overfitting to specific procedural patterns. + +.. figure:: ../_static/experimental-features/trail/curriculum.jpeg + :align: center + :figwidth: 95% + :alt: Curriculum + + Figure 2: Multiple terrain patches are instantiated across a uniform grid to construct the learning environment. + + + +Module Contents +--------------- + +The trail implementation is organized under ``isaaclab_contrib/terrains/trail``: + +- ``trail_terrains.py``: Core mesh-generation routines. Main entry point: + ``mesh_trail_segment(difficulty, cfg)``, which returns a generated mesh and its origin. +- ``trail_cfg.py``: Ready-to-use terrain presets (e.g. ``WavesCfg``, ``JumpsCfg``, ``StonesCfg``, + ``RootsCfg``, ``RampsCfg``, ``SlalomCfg``). Presets extend ``TrailBaseCfg`` and define profile, + sweep, and object-parameter ranges. +- ``elements/``: Reusable low-level building blocks: + + - ``object_profiles.py``: 2D object profiles for skill elements (waves, ramps, roots, etc.). + - ``sweeping_paths.py``: Sweep-path functions that turn 2D profiles into 3D geometry. + - ``trail_profiles.py``: Trail cross-section profiles. + - ``trail_walls.py``: Parameterization of the left and right trail walls. + - ``decoration_functions.py``: Decorative object generation (trees, roots, rocks, etc.). + - ``terrain_functions.py``: Terrain disturbance functions (sine patterns, noise, etc.). + - ``roll_functions.py``: Trail roll angle functions relative to the heading axis. + +- ``utils/``: Shared utilities across the terrain generator: + + - ``colors.py``: Color palettes and color helpers. + - ``numpy_arrays.py``: Helper functions based on NumPy parameterization. + - ``transformations.py``: Pose and transform helpers. + - ``trimesh_utils.py``: Mesh cleaning and repair helpers. + - ``math.py``: Math helper functions. + +- ``examples/``: Example code and ready-to-use configurations: + + - ``trails.py``: Trail terrain configurations ready for plug-and-play. + +Decoration functions extract the ``decoration_elements.zip`` archive and build ``.glb`` assets within +the system temporary directory (under ``/isaaclab_contrib/trail/``). Depending on the setup, +two ``.glb`` files may exist: one detailed for deployment/rendering and one simplified for training. +If any decoration objects or preprocessing functions are changed, these cached ``.glb`` files in the temporary directory must be manually deleted to force a recomputation. + +Available Terrain Presets +------------------------- + +The trail library provides the following preset configuration classes in +``isaaclab_contrib.terrains.trail.trail_cfg``: + +- ``WavesCfg``: Repeating waves as can be found on pump tracks. +- ``JumpsCfg``: Jump features with optional gaps/plateaus, which can be found on mountain-bike parks. +- ``StonesCfg``: Rock/grave patches, modeling extended rock beds. +- ``RootsCfg``: Root-like patches, modeling forest paths with root networks. +- ``RampsCfg``: Single ramp obstacles. +- ``MultipleRampsCfg``: Parallel multi-lane ramp sets (this is a typical feature of the Allmend trail in Zurich). +- ``SinusoidalCurvesCfg``: S-shaped sinusoidal trail curves. +- ``DropCurvesCfg``: Vertical sinusoidal drops/pits. +- ``WingCurvesCfg``: Circular wing-style turns. +- ``SkinnyCfg``: Narrow beam-style sections. +- ``Loops360Cfg``: 360-degree loop structures. +- ``StairsCfg``: Stair-step terrain sections as can be found on hiking paths. +- ``SlalomCfg``: Pole/slalom obstacle course. + +The image below shows some preset examples. + +.. figure:: ../_static/experimental-features/trail/presets.jpeg + :align: center + :figwidth: 95% + :alt: Preset trail terrain examples + + Figure 3: Each trail segment may contain one or more skill elements of a unique type, such as + stairs, pump-track waves, jumps, drops, rocks, curves, skinnies, roots, and ramps. + +Key Concepts +------------ + +- ``mesh_trail_segment(difficulty, cfg)``: Core generation function. The ``difficulty`` parameter + is a float in ``[0.0, 1.0]`` that linearly interpolates curriculum parameters between ``cp0`` + (easy) and ``cp1`` (hard). It is driven by Isaac Lab's terrain curriculum as the agent advances. + Difficulty values greater than ``1.0`` are not supported as they may cause division-by-zero + errors depending on the object configuration. + +- ``TrailBaseCfg`` and Presets: All terrain configs extend ``TrailBaseCfg`` (which extends Isaac + Lab's ``SubTerrainBaseCfg``). ``TrailBaseCfg`` controls surroundings, objects, trail shape, roll + angles, decorations, and coloring. Ready-made presets extend ``TrailBaseCfg`` with defaults and + expose parameters that distinguish each terrain type. + +- Control Points ``cp0`` / ``cp1``: These are ``ObjectParameters`` instances defining object + dimensions at the easiest and hardest ends of the curriculum. The interpolated value at generation + time is computed using: + + .. math:: + + \text{value} = \text{cp0} + \text{difficulty} \times (\text{cp1} - \text{cp0}) + + Each control point field can be a fixed value or a ``(min, max)`` range: + + +------------+-------------------------------------------------------------------------+ + | Field | Description | + +============+=========================================================================+ + | ``length`` | Object length along the trail heading direction [m] | + +------------+-------------------------------------------------------------------------+ + | ``width`` | Trail width at this curriculum point [m] | + +------------+-------------------------------------------------------------------------+ + | ``params`` | Dict of additional object-specific parameters (e.g., ``height``) | + +------------+-------------------------------------------------------------------------+ + + +Semantic Color Regions & MDP Integration +---------------------------------------- + +Semantic information is explicitly stored in vertex colors (``visual.vertex_colors``). The exact +RGB values can be chosen arbitrarily provided the mapping between colors and semantic labels is +bijective. + +Default semantic color assignments: + ++---------------------+-------------------+-------------------------------+ +| Region | Description | Default Color | ++=====================+===================+===============================+ +| ``col_trail`` | Trail surface | Brown (HSV) | ++---------------------+-------------------+-------------------------------+ +| ``col_trail_object``| Objects on trail | Brown (HSV) | ++---------------------+-------------------+-------------------------------+ +| ``col_floor`` | Floor beside trail| Green (HSV) | ++---------------------+-------------------+-------------------------------+ +| ``col_start`` | Starting platform | Yellow (RGB ``1, 1, 0``) | ++---------------------+-------------------+-------------------------------+ +| ``col_goal`` | Ending platform | Magenta (RGB ``1, 0, 1``) | ++---------------------+-------------------+-------------------------------+ + +Override any of these with a ``ColorParameters`` instance using HSV or RGB values. + +Setting ``visual_material=None`` in ``TerrainImporterCfg`` preserves the vertex colors for +rendering. A color-sensitive raycaster can extract these semantic labels directly from the mesh to +shape MDP rewards: + +- **Spawn location**: Place the agent on the starting platform (``col_start``). +- **Path following**: Reward agents for remaining on the trail surface (``col_trail``). +- **Success criteria**: Advance curriculum level when the agent reaches the goal platform (``col_goal``). +- **Obstacle interaction**: Reward or penalize agents for contacting obstacles (``col_trail_object``). + + +Example Terrain Generator Config +-------------------------------- + +.. code-block:: python + + from isaaclab_contrib.terrains.trail.trail_cfg import ColorParameters, ObjectParameters as OP, SlalomCfg, StonesCfg + + from isaaclab.terrains.terrain_generator_cfg import TerrainGeneratorCfg + from isaaclab.utils.configclass import configclass + + + @configclass + class TrailTerrainGeneratorCfg(TerrainGeneratorCfg): + curriculum = True + size = (50.0, 40.0) # length, width [m] + border_width = 0.0 # set to zero as handled internally + border_height: float = 0.0 # set to zero as handled internally + num_rows = 10 # terrain difficulty levels + num_cols = 100 # terrain variants per level + difficulty_range = (0.0, 1.0) # 0.0 easiest, 1.0 hardest + use_cache = False # set True to reuse generated meshes across runs once stable + + + TRAIL_CFG = TrailTerrainGeneratorCfg( + sub_terrains={ + "stones": StonesCfg( + proportion=1.0, + length_between_objects=(0.0, 0.2), + length_between_platform_and_object=(2.0, 4.0), + cp0=OP(length=(0.2, 0.4), width=2.0, params={"height": 0.0}), + cp1=OP(length=(0.4, 0.8), width=(0.9, 1.5), params={"height": (0.01, 0.1)}), + ), + "slalom": SlalomCfg( + proportion=1.0, + length_between_objects=(1.0, 3.5), + length_between_platform_and_object=(1.0, 3.0), + cp0=OP(length=0.4, width=3.5, params={"height": 1.0, "rel_dist_from_center": (1.0, 1.0)}), + cp1=OP( + length=(0.05, 0.05), + width=(1.5, 2.0), + params={"height": (0.8, 1.0), "rel_dist_from_center": (0.0, 1.0)}, + ), + col_trail_object=ColorParameters(hsv={"r": 0.0, "g": 0.0, "b": 1.0}), + col_trail=ColorParameters(hsv={"r": 1.0, "g": 0.0, "b": 0.0}), + col_floor=ColorParameters(hsv={"r": 0.0, "g": 0.0, "b": 0.0}), + col_start=ColorParameters(hsv={"r": 1.0, "g": 1.0, "b": 0.0}), + col_goal=ColorParameters(hsv={"r": 1.0, "g": 0.0, "b": 1.0}), + ), + }, + ) + + +Quick Scene Integration Example +------------------------------- + +.. code-block:: python + + from isaaclab.scene import InteractiveSceneCfg + from isaaclab.terrains import TerrainImporterCfg + from isaaclab.utils.configclass import configclass + + from isaaclab_contrib.terrains.trail.examples.trails import TRAIL_CFG + + + @configclass + class TrailSceneCfg(InteractiveSceneCfg): + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=TRAIL_CFG, + visual_material=None, # None preserves RGB vertex colors for visualization/raycasting + debug_vis=False, + ) + + +Run Without a Policy +-------------------- + +A manager-based RL environment is available and can be launched with a zero-action agent: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/environments/zero_agent.py \ + --task IsaacContrib-Velocity-Trail-AnymalC \ + --num_envs 4 + + +Testing +------- + +Unit tests for trail components can be executed with pytest: + +.. code-block:: bash + + python -m pytest source/isaaclab_contrib/test/terrains/test_trail.py diff --git a/pyproject.toml b/pyproject.toml index f7fd71b6ed39..9fee0b6d5a90 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -48,6 +48,7 @@ dependencies = [ "hidapi>=0.14.0", "gymnasium>=1.2.0", "trimesh>=4.6.8", # lower bound inherited from newton[importers] + "manifold3d>=3.0.1", "pyglet>=2.1.6,<3", "transformers==4.57.6", "einops", diff --git a/source/isaaclab_contrib/changelog.d/fjenelten-feature-trail_library_migration.rst b/source/isaaclab_contrib/changelog.d/fjenelten-feature-trail_library_migration.rst new file mode 100644 index 000000000000..69430dae919d --- /dev/null +++ b/source/isaaclab_contrib/changelog.d/fjenelten-feature-trail_library_migration.rst @@ -0,0 +1,9 @@ +Added +^^^^^ + +* Added trail terrains under ``isaaclab_contrib.terrains.trail``. + +Changed +^^^^^^^ + +* README.md: Updated the README to reflect the new trail terrains. diff --git a/source/isaaclab_contrib/docs/README.md b/source/isaaclab_contrib/docs/README.md index d6d5a43c9d7b..9bea4014ba47 100644 --- a/source/isaaclab_contrib/docs/README.md +++ b/source/isaaclab_contrib/docs/README.md @@ -30,6 +30,10 @@ It uses the Taxim model from [Si et al., 2022](https://arxiv.org/abs/2109.04027) See the [TacSL Tactile Sensor](#tacsl-tactile-sensor-detailed) section below for detailed documentation. +### Trail Library + +The Trail library is a modular framework for generating parameterizable mountain-bike trails. See [this section](#trail-library-detailed) for more information. + ## Extension Structure The extension follows Isaac Lab's standard package structure: @@ -45,6 +49,8 @@ isaaclab_contrib/ │ └── actions/ # Action terms ├── sensors/ # Contributed sensor classes │ └── tacsl_sensor/ # TacSL tactile sensor implementation +├── terrains/ # Contributed sensor classes +│ └── trail/ # Trail library └── utils/ # Utility functions and types ``` @@ -472,6 +478,198 @@ uv run python scripts/demos/sensors/tacsl_sensor.py \ --- +## Trail Library (Detailed) + +Each terrain patch comprises four components: a starting platform, a final platform, a connecting trail segment, and distinct skill elements. The resulting terrain is represented as a single mesh using the trimesh library, which models geometry as triangles and stores visual information within the RGB channels. + +A trail segment is generated by first defining a cross-section polygon with counter-clockwise ordered vertices. This polygon is then swept along a sweeping path, a sequence of knot-points (x, y, z and yaw) connecting the two platforms. To form a simple ground floor around the trail, the vertices on the left and right borders are expanded outward. Each trail segment may contain one or more skill elements of a unique type, such as ramps, skinnies, roots, rocks, or waves. Each element is parameterized by a uniform distribution over its dimensions. + +The generated terrains can easily be embedded into the existing Terrain Generator. The parameterization of the skill elements scales linearly (other maps may be defined as well) with the terrain level, allowing to employ a classical terrain curriculum. + +The terrain mesh is systematically randomized by overlaying the vertices with a sequence of geometric disturbances. Additionally, decorative elements such as trees, roots, or stones can be procedurally placed adjacent to the trail. These perturbations break predictable structural patterns, preventing overfitting to specific procedural patterns. + +Semantic information is explicitly stored in the vertex colors. The exact RGB values can be chosen arbitrarily, provided the mapping from RGB to semantic label remains bijective. The semantic information can be easily retrieved by a raycaster, which maps the RGB color back into semantic labels. This allows to shape the reward landscape using local semantics surrounding the agent. + +
+ +### Module Contents + +- `trail_terrains.py` + Core mesh-generation routines. Main entry point: + `mesh_trail_segment(difficulty, cfg)`, which returns a generated mesh and its origin. + +- `trail_cfg.py` + Ready-to-use terrain presets (for example `WavesCfg`, `JumpsCfg`, `StonesCfg`, + `RootsCfg`, `RampsCfg`). Presets extend `TrailBaseCfg` and define profile, + sweep, and object-parameter ranges. + +- `elements/` + Reusable low-level building blocks: + - `object_profiles.py`: 2D object profiles for skill elements (waves, ramps, roots, etc.) + - `sweeping_paths.py`: sweep-path functions that turn 2D profiles into 3D geometry, used to generate the trail and the skill elements. + - `trail_profiles.py`: trail cross-section profiles + - `trail_walls.py`: parametrization of the left and right trail-wall + - `decoration_functions.py`: decorative-object generation used to place trees, roots, rocks, etc in the local surrounding of the trial + - `terrain_functions.py`: terrain disturbance functions (sine patterns, noise, etc.) + - `roll_functions.py`: trail roll angle (absolute relative to heading axis) functions + +- `utils/` + Shared utilities used across the terrain generator: + - `colors.py`: color palettes and color helpers + - `numpy_arrays.py`: helpers functions based on numpy parametrization + - `transformations.py`: pose and transform helpers + - `trimesh_utils.py`: mesh cleaning and repair helpers + - `math.py`: math helper functions +- `examples/` + Contains example code: + - `trails.py`: trail terrain configuration used in the publication, ready for plug-and-play + +Decoration functions extract the `decoration_elements.zip` archive and build `.glb` assets within the system temporary directory (under `/isaaclab_contrib/trail/`). Depending on the setup, there might exist two `.glb` files: one detailed for deployment/rendering and one simplified for training. If any decoration objects or pre-processing functions are changed, all `.glb` files in the temporary directory need to be manually deleted to force a recomputation. + +### Available Terrains + +The trail library provides these terrain configuration classes in +`trail_cfg.py`: + +- `WavesCfg`: Repeating waves as can be found on pump tracks. +- `JumpsCfg`: Jump features with optional gaps/plateaus, which can be found on mountain-bike parks. +- `StonesCfg`: Rock/grave patches, modeling extended rock beds. +- `RootsCfg`: Root-like patches, modeling forest paths with root networks. +- `RampsCfg`: Single ramp obstacles. +- `MultipleRampsCfg`: Parallel multi-lane ramp sets (this is a typical feature of the Allmend trail in Zurich). +- `SinusoidalCurvesCfg`: S-shaped sinusoidal trail curves. +- `DropCurvesCfg`: Vertical sinusoidal drops/pits. +- `WingCurvesCfg`: Circular wing-style turns. +- `SkinnyCfg`: Narrow beam-style sections. +- `Loops360Cfg`: 360-degree loop structures. +- `StairsCfg`: Stair-step terrain sections as can be found on hiking paths. +- `SlalomCfg`: Pole/slalom obstacle course. + +New terrains can be added following the concept implied by the implementation. + +### Key Concepts + +- `mesh_trail_segment(difficulty, cfg)` -- The core generation function. `difficulty` is a float in `[0.0, 1.0]` that linearly interpolates all curriculum parameters between their `cp0` (easy) and `cp1` (hard) values. It is driven by Isaac Lab's terrain curriculum, which increases `difficulty` +as the agent advances and builds up its skills. + +- `TrailBaseCfg` and presets -- All terrain configs extend `TrailBaseCfg` (which itself extends Isaac Lab's `SubTerrainBaseCfg`). The `TrailBaseCfg` class controls surroundings, objects, trail shape, roll angles, decorations, and coloring. Ready-made presets (`WavesCfg`, `JumpsCfg`, +`StonesCfg`, `RootsCfg`, `RampsCfg`, `SlalomCfg`) extend `TrailBaseCfg` with +defaults and expose only the parameters that distinguish each terrain type. + +- Control points `cp0` / `cp1` -- These are `ObjectParameters` instances that define object dimensions at the **easy** and **hard** ends of the curriculum, respectively. Each field may be a fixed value or a `(min, max)` tuple from which a value is sampled per terrain patch: + + | Field | Description | + |-------|-------------| + | `length` | Object length along the trail heading direction [m] | + | `width` | Trail width at this curriculum point [m] | + | `params` | Dict of additional object-specific parameters (e.g. `height`) | + + By default, at generation time, the interpolated value is computed using the relation `cp0 + difficulty * (cp1 - cp0)`. + +#### Semantic color regions + +Vertex colors encode region type. The default color assignments are: + +| Region | Color | Default | +|--------|-------|---------| +| `col_trail` | Trail surface | Brown (HSV) | +| `col_trail_object` | Objects on trail | Brown (HSV) | +| `col_floor` | Floor beside trail | Green (HSV) | +| `col_start` | Starting platform | Yellow (RGB `1,1,0`) | +| `col_goal` | Ending platform | Magenta (RGB `1,0,1`) | + +Override any of these with a `ColorParameters(hsv={...})` or `ColorParameters(hsv={"r":…,"g":…,"b":…})` instance. + +### Example Usage + +Trail terrain generation integrates directly with the Isaac Lab terrain generator. +Below is an example definition of a terrain-generator config class. `cp0` sets object +dimensions at difficulty 0 (**easiest**) and `cp1` at difficulty 1 (**hardest**). It is not possible to set a difficulty larger than 1 as, depending on the trail objects,m this might cause division by zero. + +```py +from isaaclab_contrib.terrains.trail.trail_cfg import ObjectParameters as OP +from isaaclab_contrib.terrains.trail.trail_cfg import StonesCfg + +from isaaclab.terrains.terrain_generator_cfg import TerrainGeneratorCfg +from isaaclab.utils.configclass import configclass + +@configclass +class TrailTerrainGeneratorCfg(TerrainGeneratorCfg): + curriculum = True + size = (50.0, 40.0) # length, width + border_width = 0.0 # set this to zero as it is not used + border_height: float = 0.0 # set this to zero as it is not used + num_rows = 10 # terrain difficulty levels + num_cols = 100 # terrain variants per level + difficulty_range = (0.0, 1.0) # 0.0 easiest, 1.0 hardest + use_cache = False # set True to reuse generated meshes across runs (safe once cfg is stable) + +TRAIL_CFG = TrailTerrainGeneratorCfg( + sub_terrains={ + "stones": StonesCfg( + proportion=1.0, + length_between_objects=(0.0, 0.2), + length_between_platform_and_object=(2.0, 4.0), + cp0=OP(length=(0.2, 0.4), width=2.0, params={"height": 0.0}), + cp1=OP(length=(0.4, 0.8), width=(0.9, 1.5), params={"height": (0.01, 0.1)}), + ) + }, +) +``` + +A manger based RL environment is set up in isaaclab_tasks/contib/trail and can be run with `./isaaclab.sh -p scripts/environments/zero_agent.py --task IsaacContrib-Velocity-Trail-AnymalC --num_envs 4` + +### Semantic Embedding + +The colors stored in `visual.vertex_colors` can be visualized by setting `visual_material=None` in the `TerrainImporterCfg`. Those colors can also be read directly with a color-sensitive ray caster. In this case, MDP components can be designed to digest semantic information extracted directly from the mesh. For instance, by coloring the initial platform, the final platform, the trail, and the trail objects uniquely, it is possible to +- span the agent at the initial platform, +- reward agents that follow the trail, +- upgrade agents to the next level that reach the final platform, +- penalize/reward agents that touch/overcome certain objects or reach certain checkpoints, and +- design a terrain curriculum that upgrades the agents upon reaching the final platform. + +As an example, a slalom-like environment may be created using the following terrain config +```py +sub_terrains["slalom"] = SlalomCfg( + proportion=1.0, + length_between_objects=(1.0, 3.5), + length_between_platform_and_object=(1.0, 3.0), + cp0=OP(length=0.4, width=3.5, params={"height": 1.0, "rel_dist_from_center": (1.0, 1.0)}), + cp1=OP( + length=(0.05, 0.05), width=(1.5, 2.0), params={"height": (0.8, 1.0), "rel_dist_from_center": (0.0, 1.0)} + ), + col_trail_object=ColorParameters(hsv={"r": 0.0, "g": 0.0, "b": 1.0}), + col_trail=ColorParameters(hsv={"r": 1.0, "g": 0.0, "b": 0.0}), + col_floor=ColorParameters(hsv={"r": 0.0, "g": 0.0, "b": 0.0}), + col_start=ColorParameters(hsv={"r": 1.0, "g": 1.0, "b": 0.0}), + col_goal=ColorParameters(hsv={"r": 1.0, "g": 0.0, "b": 1.0}), +) +``` + +### Quick Start + +Attach the trail terrain generator to a scene of manager-based RL environment as follows +```py +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils.configclass import configclass + +from isaaclab_contrib.terrains.trail.examples.trails import TRAIL_CFG # import the example trail config class + +@configclass +class TrailSceneCfg(InteractiveSceneCfg): + + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=TRAIL_CFG, + visual_material=None, # set to None if you want to see the RGB colors defined in the mesh + debug_vis=False, + ) +``` + +
+ ## Testing The extension includes comprehensive unit tests for all contributed components: @@ -481,6 +679,10 @@ The extension includes comprehensive unit tests for all contributed components: python -m pytest source/isaaclab_contrib/test/assets/test_multirotor.py python -m pytest source/isaaclab_contrib/test/actuators/test_thruster.py python -m pytest source/isaaclab_contrib/test/assets/test_drone_geometric_controllers.py + +# Test trail components +python -m pytest source/isaaclab_contrib/test/terrains/test_trail.py + # Run all contrib tests python -m pytest source/isaaclab_contrib/test/ ``` diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/decoration_elements.zip b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/decoration_elements.zip new file mode 100644 index 000000000000..3558606bb9c1 Binary files /dev/null and b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/decoration_elements.zip differ diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/decoration_functions.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/decoration_functions.py new file mode 100644 index 000000000000..2901a1409671 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/decoration_functions.py @@ -0,0 +1,182 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. +from __future__ import annotations + +""" +This file contains functions to generate, load, and process decorative elements +(for example, trees and rocks) in the scene. Assets can be downloaded from: +https://www.turbosquid.com/Search/3D-Models/low-poly/ + +Most objects were downloaded from these sources: + > Trees: https://www.turbosquid.com/3d-models/3d-low-poly-trees-1431822 + > Rocks: https://www.turbosquid.com/3d-models/low-poly-stones-3d-2288912 +""" +import colorsys +import os +import random +import shutil +import tempfile +import zipfile +from typing import TYPE_CHECKING, Literal + +import numpy as np +import trimesh + +from ..utils import colors, transformations, trimesh_utils +from ..utils.math import sample + +if TYPE_CHECKING: + from ..trail_cfg import TrailBaseCfg + + +def _decoration_cache_path() -> str: + """Return the writable directory used for extracted and generated decoration assets.""" + return os.path.join(tempfile.gettempdir(), "isaaclab_contrib", "trail", "decoration_elements") + + +def generated_path(training: bool) -> str: + """Return the path of the generated decoration GLB file.""" + return os.path.join( + tempfile.gettempdir(), + "isaaclab_contrib", + "trail", + "list_of_objects_" + ("training" if training else "deployment") + ".glb", + ) + + +def generate(training: bool): + """Helper function that loads all available decorative elements, processes them, and stores them into a single glb + file. + + Args: + training: If true, simplifies the decorative objects. + """ + # Selection of decorative elements with their target triangle counts + detail_factor = 1.0 if training else 5.0 + objects = [ + # winter and summer trees + ("trees/evergreen/pine1.obj", 300), + ("trees/evergreen/pine2.obj", 900), + ("trees/evergreen/pine3.obj", 600), + ("trees/evergreen/pine4.obj", 250), + # summer trees + ("trees/summer/tree1.obj", 300), + ("trees/summer/tree2.obj", 300), + ("trees/summer/tree3.obj", 400), + ("trees/summer/tree4.obj", 800), + ("trees/summer/tree5.obj", 250), + ("trees/summer/tree6.obj", 500), + # winter trees + ("trees/winter/tree1.obj", 250), + # roots + ("roots/root1.obj", 100), + # rocks + ("rocks/rock1.obj", 60), + ("rocks/rock2.obj", 50), + ] + + scene = trimesh.Scene() + file_path = os.path.dirname(os.path.realpath(__file__)) + decoration_elements_path = _decoration_cache_path() + decoration_elements_zip_path = os.path.join(file_path, "decoration_elements.zip") + + # Extract local asset bundle on demand when the folder is not yet present. + if not os.path.isdir(decoration_elements_path): + if not os.path.isfile(decoration_elements_zip_path): + raise FileNotFoundError( + f"Neither '{decoration_elements_path}' nor '{decoration_elements_zip_path}' exists." + ) + os.makedirs(os.path.dirname(decoration_elements_path), exist_ok=True) + with zipfile.ZipFile(decoration_elements_zip_path, "r") as zip_ref: + zip_ref.extractall(os.path.dirname(decoration_elements_path)) + + if not os.path.isdir(decoration_elements_path): + raise FileNotFoundError( + f"Expected extracted folder '{decoration_elements_path}' was not created from " + f"'{decoration_elements_zip_path}'." + ) + + for id, (object_name, num_target_triangles) in enumerate(objects): + # Load object + object_mesh = trimesh.load(os.path.join(decoration_elements_path, object_name)) + # Simplify mesh + object_mesh = trimesh_utils.fix_mesh(object_mesh) + object_mesh = trimesh_utils.simplify_mesh( + mesh=object_mesh, + method="absolute_quadric_decimation", + parameter=round(num_target_triangles * detail_factor), + ) + print( + "Generate decorative object", + object_name, + "with", + object_mesh.vertices.shape[0], + "vertices", + ) + # Add object to the scene + scene.add_geometry(object_mesh, geom_name=object_name) + # Store as GLB file + scene.export(generated_path(training)) + if os.path.isdir(decoration_elements_path): + shutil.rmtree(decoration_elements_path) + + +def load_object_mesh( + list_of_objects: dict[str, list[trimesh.Trimesh]], + cfg: TrailBaseCfg, + object_dist: dict[Literal["evergreen", "summer", "winter", "roots", "rocks"], float], +) -> trimesh.Trimesh: + """Load a decorative object into the scene near the path center. + + Args: + list_of_objects: Mapping from object type to lists of decorative meshes. + cfg: Configuration for the sub-terrain. + object_dist: Probability distribution over object types. + + Returns: + The selected object's mesh. + """ + # Ignore zero-probability entries and normalize active weights. + if not object_dist: + raise ValueError("object_dist must contain at least one entry with probability > 0.") + choices = list(object_dist.keys()) + weights = np.array(list(object_dist.values()), dtype=float) + weights = weights / weights.sum() + object_type = np.random.choice(choices, p=weights) + object_mesh = random.choice(list_of_objects[object_type]).copy() + + # Randomize scale + scale = sample((0.4, 1.0)) # scale along all axes + scale_factors = np.random.uniform(0.9, 1.1, 3) * scale # scales along individual axes + T = transformations.scale(vec=scale_factors) + # Randomize orientation + if object_type == "rocks": + T_rot = trimesh.transformations.quaternion_matrix(trimesh.transformations.random_quaternion()) + T = trimesh.transformations.concatenate_matrices(T, T_rot) + else: + T_yaw = transformations.yaw(angle=sample((0.0, 2.0 * np.pi))) + T = trimesh.transformations.concatenate_matrices(T, T_yaw) + # Randomize vertex colors + color_rgb = object_mesh.visual.vertex_colors[:, 0:3] / 255.0 + color_hsv = np.array([colorsys.rgb_to_hsv(*rgb) for rgb in color_rgb]) + color_hsv[:, 0] += np.random.uniform(-0.05, 0.05) + color_hsv[:, 0] = color_hsv[:, 0] % 1.0 + color_hsv[:, 1] *= np.random.uniform(0.9, 1.1) + color_hsv[:, 2] *= np.random.uniform(0.4, 1.0) + color_hsv[:, 1] = np.clip(color_hsv[:, 1], a_min=0.0, a_max=1.0) + object_mesh.visual.vertex_colors = colors.hsv_to_rgb(color_hsv) + object_mesh.apply_transform(T) + # Cut object vertices above the configured height + if cfg.cut_objects_above is not None: + object_mesh = trimesh_utils.cut_above(mesh=object_mesh, height=cfg.cut_objects_above) + # Replace mesh with a convex approximation if requested + if cfg.convex_approx: + object_mesh_convex = object_mesh.convex_hull + median_rgb = np.median(object_mesh.visual.vertex_colors, axis=0)[0:3] + object_mesh_convex.visual.vertex_colors[:, 0:3] = np.tile(median_rgb, (object_mesh_convex.vertices.shape[0], 1)) + object_mesh = object_mesh_convex + return object_mesh diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/object_profiles.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/object_profiles.py new file mode 100644 index 000000000000..646f2490f186 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/object_profiles.py @@ -0,0 +1,239 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +""" +This module contains functions to generate 2D profiles that represent the +shape of trail objects. Trail objects are placed inside the trail to simulate +obstacles. + +When creating new objects, consider the following: + * Objects are defined in either the xz or yz plane. + * The x (or y) axis points to the right and the z axis points downward + (z coordinates are flipped). + * The object's anchor point is at (x/y, z) = (0, 0). + * The object is parameterized as a NumPy array with shape (N, 2). + * The outline of the object is not closed (i.e., the first and last point + are not identical). + + x/y=0 + +--------+----------------------> x or y + | | + | | + z=0+--------+ (beginning of object) + | + | + z +""" + +import math +import random +from typing import Literal + +import numpy as np + +from ..utils.math import sample + + +def curved_ramp_profile( + length: float, + height: float, + num_segments: int, + type: Literal["cos", "half-cos", "rand"], +) -> np.ndarray: + """Helper to generate a cosine-shaped ramp profile. + + Args: + length: Length of the ramp, from entry point to exit point [m]. + height: Height of the element, reached at the peak point [m]. + num_segments: Number of segments used to parametrize the surface. + type: Type of the ramp. Can be: + * ``"cos"``: smooth connection to floor and top + * ``"half-cos"``: smooth connection to floor only + * ``"rand"``: randomly picks one of the above options + + Returns: + A polygon specifying the profile outline, parameterized as a NumPy array + with shape (N, 2). + """ + s = np.linspace(0.0, 1.0, (num_segments if length > 0.0 else 2)) + xi = s * length + if type == "rand": + type = random.choice(["cos", "half-cos"]) + if type == "cos": + zi = 0.5 * (np.cos(np.pi * s) - 1.0) * height + elif type == "half-cos": + zi = (np.cos(0.5 * np.pi * s) - 1.0) * height + else: + raise RuntimeError("Unknown type argument.") + return np.stack([xi, zi], axis=1) + + +def wave_profile( + length: float, + height: float, + num_segments: int, + platform_length: float = 0.0, + gap: bool | tuple[bool, bool] = False, + exponent: int | tuple[int, int] = 2, + type: Literal["cos", "half-cos", "rand"] = "cos", +) -> np.ndarray: + """Helper to generate a cosine-shaped wave profile consisting of two ramps connected by an optional middle platform. + + Args: + length: Length of each ramp, from entry point to peak point [m]. + height: Height of the element at the platform [m]. + num_segments: Number of segments used to parametrize the surface. + platform_length (optional): Platform length inserted between the two + ramps [m]. Default is 0. + gap (optional): If true, the platform is not filled (there is a gap + between the two ramps). Default is False. + exponent: Larger exponents make the gap profile approach a square; + a value of 1 yields a sinusoidal shape. + type (optional): Type of the ramp. Default is "cos". Can be: + * ``"cos"``: smooth connection to floor and top + * ``"half-cos"``: smooth connection to floor only + * ``"rand"``: randomly picks one of the above options + + Returns: + A polygon specifying the profile outline, parameterized as a NumPy + array with shape (N, 2). + """ + # Generate profile for ramp + ramp_up = curved_ramp_profile(length=length, height=height, num_segments=num_segments, type=type) + + # Extend the ramp to a wave by appending an inverted ramp on the other side + ramp_down = ramp_up.copy() + ramp_down[:, 1] = np.flip(ramp_up[:, 1]) + ramp_down[:, 0] += length + platform_length # insert platform + + if platform_length == 0.0: + s = np.linspace(1.0, 0.0, 2 * num_segments) + floor = np.stack([s * (2.0 * length), s * 0.0], axis=1) + return np.vstack([ramp_up, ramp_down, floor[1:-1, :]]) + + # Floor + s = np.linspace(1.0, 0.0, 3 * num_segments) + floor = np.stack([s * (2.0 * length + platform_length), s * 0.0], axis=1) + + # Add gap between the two ramps + s = np.linspace(0.0, 1.0, 2 * num_segments) + xi = s * (ramp_down[0, 0] - ramp_up[-1, 0]) + ramp_up[-1, 0] + if sample(gap): + rel_platform_h = sample((0.05, 0.2)) + zi = ( + -((0.5 * (np.cos(2.0 * np.pi * s) + 1.0)) ** sample(exponent)) * (1.0 - rel_platform_h) * height + - rel_platform_h * height + ) + else: + zi = s * 0.0 - height + gap = np.stack([xi, zi], axis=1) + return np.vstack([ramp_up, gap[1:-1, :], ramp_down, floor[1:-1, :]]) + + +def root_profile(length: float, height: float, num_segments: int, exponent: int | tuple[int, int]) -> np.ndarray: + """Helper function to generate a root profile. + + Args: + length: length of the root profile. + height: the height of the root profile. + num_segments: number of segments used to parametrize profile shape. + exponent: the larger the exponent is, the more the root profile approaches a square. + A value of 1 means a sinusoidal shape. + + Returns: + a polygon specifying the profile outline, parameterized as a NumPy array. + """ + s = np.linspace(0.0, 1.0, num_segments) + xi = s * length + zi = (((1.0 - np.sin(np.pi * s)) ** sample(exponent)) - 1.0) * height + return np.stack([xi, zi], axis=1) + + +def ramp_profile(length: float, height: float, num_segments: int, elevation: float = 0.0) -> np.ndarray: + """Helper function to generate a ramp profile. + + Args: + length: length of the ramp profile. + height: the height of the ramp profile. + num_segments: number of segments used to parametrize profile shape. Must > 1. + elevation: additional support height below the ramp [m]. If > 0, + the ramp is shifted upward and a box-like support is added below it. + + Returns: + a polygon specifying the profile outline, parameterized as a NumPy array. + """ + # ramp + s = np.linspace(0.0, 1.0, num_segments) + xi = s * length + zi = -height + s * height - elevation + ramp = np.stack([xi, zi], axis=1) + # floor + s = np.linspace(1.0, 0.0, num_segments) + xi = s * length + zi = s * 0.0 + floor = np.stack([xi, zi], axis=1) + if elevation > 0.0: + # Add the top-right corner of the support box explicitly so the profile + # contains a vertical face at x=length. + support_corner = np.array([[length, 0.0]]) + return np.vstack([ramp, support_corner, floor[1:, :]]) + # combine + return np.vstack([ramp, floor[1:, :]]) + + +def box_profile(length: float, height: float) -> np.ndarray: + """Helper function to generate a box profile. + + Args: + length: length of the box profile. + height: the height of the box profile. + + Returns: + a polygon specifying the profile outline, parameterized as a NumPy array. + """ + xi = [0.0, 0.0, length, length] + zi = [0.0, -height, -height, 0.0] + return np.stack([xi, zi], axis=1) + + +def stair_profile(length: float, height: float, step_width: float) -> np.ndarray: + """Helper function to generate a stair profile. + + Args: + length: length of the stair profile [m]. + height: the height of the stair profile [m]. + step_width: step width [m]. + + Returns: + a polygon specifying the profile outline, parameterized as a NumPy array. + """ + # init + xi = [0.0] + zi = [0.0] + x = 0.0 + z = 0.0 + + # compute number of steps + num_steps = math.floor(length / step_width) + if num_steps == 0: + num_steps = 1 + # recompute stair parameters to fit number of steps + step_width = length / num_steps + step_height = height / num_steps + + for _ in range(num_steps): + # add current step + xi = np.concatenate([xi, [x, x + step_width]], axis=0) + zi = np.concatenate([zi, [z - step_height, z - step_height]], axis=0) + # prepare next step + x += step_width + z -= step_height + + # close the stair profile + xi = np.concatenate([xi, [xi[-1]]], axis=0) + zi = np.concatenate([zi, [0.0]], axis=0) + return np.stack([xi, zi], axis=1) diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/roll_functions.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/roll_functions.py new file mode 100644 index 000000000000..f99f5285d4d7 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/roll_functions.py @@ -0,0 +1,89 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +""" +Utilities to compute trail roll angles along a sweeping path. + +Example usage:: + + angles = roll_function(path, params) + +where ``angles`` are the relative roll angles along the sweeping path. + +The function signature for a roll function should be:: + + def roll_function(path: np.ndarray, params: dict[str, float]) -> np.ndarray: + Args: + path: The sweeping path of the trail as an (N, M) array. + params: Additional parameters used to compute the roll angles. + + Returns: + A NumPy array of roll angles (radians) along the sweeping path. +""" + +import random + +import numpy as np + + +def lin_derivative_y(path: np.ndarray, params: dict[str, float]) -> np.ndarray: + """Return roll angles proportional to the y-derivative of the path. + + The angle at each knot is computed from the local difference in the y coordinate between consecutive path points and + clipped to the configured maximum angle. + """ + num_knots = path.shape[0] + angles = np.zeros(num_knots) + for id in range(1, num_knots, 1): + angles[id - 1] -= ( + params["gain_der_y"] * (path[id, 1] - path[id - 1, 1]) / np.linalg.norm(path[id, :] - path[id - 1, :]) + ) + return np.clip(angles, a_min=-params["max_angle"], a_max=params["max_angle"]) + + +def lin_y(path: np.ndarray, params: dict[str, float]) -> np.ndarray: + """Return roll angles proportional to the path y coordinate. + + Angles are scaled by ``params['gain_y']`` and clipped to ``[-params['max_angle'], params['max_angle']]``. + """ + return np.clip( + -params["gain_y"] * path[:, 1].squeeze(), + a_min=-params["max_angle"], + a_max=params["max_angle"], + ) + + +def sin_x(path: np.ndarray, params: dict[str, float]) -> np.ndarray: + """Return sinusoidal roll angles as a function of the x coordinate. + + The sinusoid amplitude and period are specified by ``params['Ax']`` and ``params['Tx']``. The phase is sampled + randomly per invocation. + """ + return params["Ax"] * np.sin(2.0 * np.pi * path[:, 0] / params["Tx"] + random.uniform(0.0, np.pi)) + + +def sin_s(path: np.ndarray, params: dict[str, float]) -> np.ndarray: + """Return sinusoidal roll angles as a function of path length (s). + + The cumulative path length ``s`` is computed along the knot points and used to evaluate a sinusoid with amplitude + ``params['As']`` and period ``params['Ts']``. The phase is randomized. + """ + num_knots = path.shape[0] + s = np.zeros(num_knots) + for id in range(1, num_knots, 1): + s[id] = s[id - 1] + np.linalg.norm(path[id, :] - path[id - 1, :]) + return params["As"] * np.sin(2.0 * np.pi * s / params["Ts"] + random.uniform(0.0, np.pi)) + + +def const(path: np.ndarray, params: dict[str, float]) -> np.ndarray: + """Return a constant roll angle along the full path. + + The constant angle magnitude is specified by ``params['A']`` in radians, and the sign is sampled randomly per + invocation. + """ + sign = random.choice([-1.0, 1.0]) + return np.full(path.shape[0], sign * params["A"], dtype=float) diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/sweeping_paths.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/sweeping_paths.py new file mode 100644 index 000000000000..257ad0ecf1f0 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/sweeping_paths.py @@ -0,0 +1,368 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +"""This file contains sweeping paths for trail curves and trail objects. + +When creating new curves, consider the following: + * The trail profile polygon is constructed in yz plane, and then swept along an arbitrary axis. + * For sweeping along y axis, the following coordinate system is utilized + x + ^ + | + | + y <------+ + * For sweeping along z axis, the following coordinate system is utilized + z + ^ + | + | + y <------+ +""" + +import random + +import numpy as np + +from ..utils.math import sample + + +def linear_path_y(width: float, num_segments) -> np.ndarray: + """Sweeping happens from left to right, i.e. along y axis. + + Args: + width: the width in y direction [m]. + num_segments: number of segments used to approximate the path. + + Returns: + sweeping path parametrized as numpay array. + """ + si = np.linspace(0.0, 1.0, num_segments) + xi = si * 0.0 + yi = si * width - 0.5 * width + zi = si * 0.0 + return np.stack([xi, yi, zi], axis=1) + + +def sinusoidal_z_curve_x( + length: float, + amplitude: float | tuple[float, float], + num_curves: int | tuple[int, int], + num_segments: int, +) -> np.ndarray: + """Sinusoidal curve in xz plane. + + Args: + length: Total length of curve segment in heading direction of trail [m]. + amplitude: Amplitude of the sinusoidal wave [m]. Positive means the curve is facing up wrt. the trail direction. + num_curves: Number of curves joint together. 1 means half of a sinusoidal curve on a (0,pi) interval. + num_segments: number of segments used to approximate half of a sinusoidal curve. + + Returns: + Sweeping path parametrized as numpay array with dimensions Nx3. + """ + n = sample(num_curves) + if n < 0.0: + raise RuntimeError("num_curves needs to be a positive number.") + s = np.linspace(0.0, 1.0, num_segments * n) + xi = s * length + yi = s * 0.0 + zi = 0.5 * (1.0 - np.cos(n * np.pi * s)) * sample(amplitude) + return np.stack([xi, yi, zi], axis=1) + + +def sinusoidal_y_curve_x( + length: float, + amplitude: float | tuple[float, float], + num_curves: int | tuple[int, int], + num_segments: int, +) -> np.ndarray: + """Sinusoidal curve in xy plane. + + Args: + length: Total length of curve segment in heading direction of trail [m]. + amplitude: Amplitude of the sinusoidal wave [m]. + Positive means the curve is facing left wrt. the trail direction. + num_curves: Number of curves joint together. 1 means half of a sinusoidal curve on a (0,pi) interval. + num_segments: number of segments used to approximate half of a sinusoidal curve. + + Returns: + Sweeping path parametrized as numpay array with dimensions Nx3. + """ + curve = sinusoidal_z_curve_x( + length=length, + amplitude=amplitude, + num_curves=2 * sample(num_curves), + num_segments=num_segments, + ) + return np.stack([curve[:, 0], curve[:, 2], curve[:, 1]], axis=1) # switch z with y axis + + +def circular_xy_curve_x( + length: float, + radius: float | tuple[float, float], + rel_angle: float | tuple[float, float], + slope: float | tuple[float, float], + max_wing_length: float, + num_segments: int, +) -> np.ndarray: + """This function creates three circular curves in xy plane, creating a wing like structure. + + * The first curve is a left turn if rel_angle > 0 (otherwise it is a right turn). + * A segment of zero curvature follows, called the wing. The length of this segment is computed from the length. + * The second curve is twice as large as the first one. + * Another wing segment follows, that connects the second with the third curve. + * A final curve aligns the trail back to the x axis. + * If the wings are larger than max_wing_length, the entire curve is adapted. + The total length will then be smaller then specified. + + ^ x + | + + | + | | + +-----+ | + + | + | | + + | + +-----+ | + + | + | | + y <---------------------+ + + Args: + length: Total length of curve segment in heading direction of trail [m]. + radius: The radius of each curve [m]. + rel_angle: Relative angle wrt to 90 degrees in (0,1). 1 means 90 degrees and is the maximum, 0 means 0 degrees. + If positive, the segments is curved towards the left (+y), otherwise towards the right (-y). + slope: The range of slopes of the curve [m]. + max_wing_length: If the wing length is larger then this value, + the curve will be adapted to satisfy this constraint. + num_segments: number of segments used to approximate each 90 degrees segment. + + Returns: + Sweeping path parametrized as numpay array with dimensions Nx3. + """ + # sample + radius = sample(radius) + rel_angle = sample(rel_angle) + slope = sample(slope) + max_wing_length = sample(max_wing_length) + + # extract sign from angle. + sing_angle = np.sign(rel_angle) + rel_angle = abs(rel_angle) + + # check validity of arguments + if rel_angle > 1.0: + raise RuntimeError( + "rel_angle has to be in the interval [0,1]. A value larger then this would yield backwards oriented trails." + ) + if 2.0 * radius > max_wing_length: + raise RuntimeError("Cannot satisfy max_wing_length constraints. Choose a smaller radius.") + + # compute the curve angle. + angle = 0.5 * rel_angle * np.pi + s = np.linspace(0.0, 1.0, num_segments) + + # first circle + center = [0.0, radius * sing_angle] # center of the circle + phase = 0.0 + x1 = center[0] + np.sin(angle * s + phase) * radius + y1 = center[1] - np.cos(angle * s + phase) * radius * sing_angle + z1 = s * 0.0 + + # compute position of second circle + center[0] = x1[-1] + np.sin(angle) * radius + center[1] = y1[-1] - np.cos(angle) * radius * sing_angle + + # add constant segment + wing_length = 0.5 * length - 2.0 * x1[-1] + if wing_length < 0.0: + raise RuntimeError("The wing length is smaller then zero. Reduce the radius or increase the length.") + center[0] += wing_length + center[1] += np.tan(angle) * wing_length * sing_angle + + # If constraints are violated, move the center of second circle + overshoot_wing = abs(center[1]) - (max_wing_length - radius) + if overshoot_wing > 0.0: + center[1] = np.clip(center[1], -max_wing_length + radius, max_wing_length - radius) + if rel_angle < 1.0: + center[0] -= overshoot_wing / np.tan(angle) + else: + center[0] = 2.0 * radius + + # second circle + phase += 0.5 * np.pi - angle + x2 = center[0] - np.cos(angle * s + phase) * radius + y2 = center[1] + np.sin(angle * s + phase) * radius * sing_angle + z2 = s * 0.0 + + # combine the two curves + xi = np.concatenate([x1, x2], axis=0) + yi = np.concatenate([y1, y2], axis=0) + zi = np.concatenate([z1, z2], axis=0) + + # mirror the curve along y axis (make sure the curve stops where it starts in y direction) + xi = np.concatenate([xi, xi + xi[-1]], axis=0) + yi = np.concatenate([yi, np.flip(yi)], axis=0) + zi = np.concatenate([zi, zi], axis=0) + + # add slope + zi += (xi - xi[0]) * slope + + return np.stack([xi, yi, zi], axis=1) + + +def zig_zag_yz_path_x( + length: float, + amplitude_y: float | tuple[float, float], + amplitude_z: float | tuple[float, float], + rel_knot_point: float | tuple[float, float], +) -> np.ndarray: + """Sweeping follows the direction of the trail, with two lateral/vertical peaks. + + Args: + length: the length of path measured in heading direction of the trail [m]. + amplitude_y: max overshoot in lateral direction [m]. + amplitude_z: max height above nominal ground at turning locations [m]. + rel_knot_point: location of the first knot, in (0, 0.5) + + Returns: + Sweeping path parametrized as numpay array. + """ + r = sample(rel_knot_point) + if r > 0.5: + raise RuntimeError("rel_knot_point needs to be a number between 0 and 0.5.") + return np.vstack( + [ + [length * 0.0, 0.0, 0.0], + [length * 0.05, 0.0, 0.0], + [length * r, sample(amplitude_y), sample(amplitude_z)], + [length * (1.0 - r), sample(amplitude_y), sample(amplitude_z)], + [length * 0.95, 0.0, 0.0], + [length * 1.0, 0.0, 0.0], + ] + ) + + +def sinusoidal_xz_curve_y( + width: float, + amplitude_x: float | tuple[float, float], + amplitude_z: float | tuple[float, float], + T_xz: float | tuple[float, float], + num_segments: int, +) -> np.ndarray: + """Sweeping path is defined by sinusoidal curves in x and z. + + The y direction is swept linearly. + Args: + width: max width in y direction [m]. + amplitude_x: the amplitude of the x-sine wave is sampled from this range [m]. + amplitude_z: the amplitude of the z-sine wave is sampled from this range [m]. + T_xz: wave length of the sinusoidal wave is sampled from this range [m]. + + Returns: + Sweeping path parametrized as numpay array. + """ + + def sample_sine_wave( + amplitude: float | tuple[float, float], + T_xz: float | tuple[float, float], + s: np.ndarray, + ) -> np.ndarray: + A = sample(amplitude) # sampled amplitude + N = 1.0 / sample(T_xz) # sampled number of waves + phase = random.uniform(0.0, np.pi) # sampled phase + return np.sin(N * 2.0 * np.pi * s + phase) * A + + s = np.linspace(0.0, 1.0, num_segments) + xi = sample_sine_wave(amplitude=amplitude_x, T_xz=T_xz, s=s) + yi = (s - 0.5) * width + zi = sample_sine_wave(amplitude=amplitude_z, T_xz=T_xz, s=s) + return np.stack([xi, yi, zi], axis=1) + + +def sinusoidal_exp_z_curve_y( + width: float, + stone_length: float | tuple[float, float], + stone_height: float | tuple[float, float], + distance_between_stones: float | tuple[float, float], + exponent: int | tuple[int, int], + num_segments: int, +) -> np.ndarray: + """Sweeping path is a repetition of sinusoidal curves in z. + + The y direction is swept linearly, x is hold constant. + This sweeping function may be useful to generate gravel roads. + + Args: + width: The width in y direction [m]. + stone_length: The length of the stone profile [m]. + stone_height: The height of the stone profile [m]. + distance_between_stones: Distance between two adjacent stones [m]. + exponent: the larger the exponent is, the more the stone profile approaches a square. + A value of 1 means a sinusoidal shape. + num_segments: number of segments used to approximate one wave (stone). + + Returns: + Sweeping path parametrized as numpay array. + """ + + def sample_segment( + length: float | tuple[float, float], + height: float | tuple[float, float], + exponent: tuple[int, int], + s: np.ndarray, + ) -> np.ndarray: + A = sample(height) # sampled amplitude + L = sample(length) # sampled length + exp = sample(exponent) # sampled exponent + zi = -(((1.0 - np.sin(np.pi * s)) ** exp) - 1.0) * sample(height) - A + xi = s * 0.0 + yi = s * L + return np.stack([xi, yi, zi], axis=1) + + # create first segment + s = np.linspace(0.0, 1.0, num_segments) + path = sample_segment(length=stone_length, height=stone_height, exponent=exponent, s=s) + + # add segments until the path is full + total_width = 0.0 + max_stone_length = stone_length[1] if isinstance(stone_length, tuple) else stone_length + while total_width < width - max_stone_length * 0.5: + segment = sample_segment(length=stone_length, height=stone_height, exponent=exponent, s=s) + segment[:, 1] += path[-1, 1] + sample(distance_between_stones) + path = np.vstack([path, segment]) + total_width = path[-1, 1] + # center the path in the middle of the trail + path[:, 1] -= 0.5 * total_width + return path + + +def loop_curve_x( + displacement_z: float, + radius: float, + angle: float, + num_segments: int, +) -> np.ndarray: + """Sweeping path creates a curve in the xy plane. + + The curve is swept in negative x-axis. + Args: + displacement_z: Height difference at the exit point [m]. + radius: The radius of the curve [m]. + angle: The circle is swept up to this angle [rad]. + num_segments: number of segments used to approximate one wave stone. + + Returns: + Sweeping path parametrized as numpay array. + """ + s = np.linspace(0.0, 1.0, num_segments) + center = [0.0, radius] + xi = center[0] - np.sin(angle * s) * radius + yi = center[1] - np.cos(angle * s) * radius + zi = s * displacement_z - displacement_z * 0.5 + return np.stack([xi, yi, zi], axis=1) diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/terrain_functions.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/terrain_functions.py new file mode 100644 index 000000000000..b25c921b22f5 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/terrain_functions.py @@ -0,0 +1,201 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +"""Terrain deformation primitives used by trail generation. + +Each function in this module returns a vertex displacement array ("delta") +with the same shape as the input ``vertices`` (``N x 3``). The caller applies +the deformation as: + + new_vertices = vertices + delta + +Most parameters support either a fixed value or a ``(min, max)`` tuple. +Tuple-valued parameters are sampled via :func:`sample` when the function runs. +""" + +from typing import Literal + +import numpy as np + +from ..utils.math import sample + + +def delta_z_slope_x(vertices: np.ndarray, S: float | tuple[float, float], difficulty: float) -> np.ndarray: + """Create a linear height ramp along the x-axis. + + The z displacement is computed as ``dz = (x - min_x) * S``. + + Args: + vertices: Terrain vertices with shape ``(N, 3)``. + S: Slope in ``m/m`` (float or sampled range). + difficulty: Difficulty in ``[0, 1]``. Present for API consistency. + + Returns: + Vertex displacement with shape ``(N, 3)``. + """ + min_x = np.min(vertices[:, 0]) + x = vertices[:, 0] - min_x + zeros = np.zeros(len(x)) + dz = x * sample(S) + return np.stack([zeros, zeros, dz], axis=1) + + +def delta_xyz_sin_x( + vertices: np.ndarray, + A: float | tuple[float, float], + T: float | tuple[float, float], + N: int | tuple[int, int], + difficulty: float, +) -> np.ndarray: + """Apply sinusoidal displacement in all three coordinates as a function of x. + + For each iteration and each axis ``i in {x, y, z}``, the function adds: + ``delta_i += A * sin(2*pi*(x - min_x) / T)``. + + Args: + vertices: Terrain vertices with shape ``(N, 3)``. + A: Wave amplitude in meters (float or sampled range). + T: Wave period in meters (float or sampled range). + N: Number of additive wave passes. + difficulty: Difficulty in ``[0, 1]``. Present for API consistency. + + Returns: + Vertex displacement with shape ``(N, 3)``. + """ + min_x = np.min(vertices[:, 0]) + x = 2.0 * np.pi * (vertices[:, 0] - min_x) + xyz = np.zeros_like(vertices) + num_iter = sample(N) + for _ in range(num_iter): + for dim in range(3): + xyz[:, dim] += sample(A) * np.sin(x / sample(T)) + return xyz + + +def delta_i_sin_x( + vertices: np.ndarray, + A: float | tuple[float, float], + T: float | tuple[float, float], + dim: Literal[0, 1, 2], + N: int | tuple[int, int], + difficulty: float, +) -> np.ndarray: + """Apply sinusoidal displacement in a single selected coordinate. + + For each iteration, the function adds: + ``delta[dim] += A * sin(2*pi*(x - min_x) / T + phase)``, + where ``phase`` is sampled in ``[0, pi]``. + + Args: + vertices: Terrain vertices with shape ``(N, 3)``. + A: Wave amplitude in meters (float or sampled range). + T: Wave period in meters (float or sampled range). + dim: Target coordinate index: ``0`` (x), ``1`` (y), or ``2`` (z). + N: Number of additive wave passes. + difficulty: Difficulty in ``[0, 1]``. Present for API consistency. + + Returns: + Vertex displacement with shape ``(N, 3)``. + """ + min_x = np.min(vertices[:, 0]) + x = 2.0 * np.pi * (vertices[:, 0] - min_x) + xyz = np.zeros_like(vertices) + num_iter = sample(N) + for _ in range(num_iter): + xyz[:, dim] += sample(A) * np.sin(x / sample(T) + sample((0.0, np.pi))) + return xyz + + +def delta_z_sin_xy( + vertices: np.ndarray, + A: float | tuple[float, float], + Tx: float | tuple[float, float], + Ty: float | tuple[float, float], + N: int | tuple[int, int], + difficulty: float, +) -> np.ndarray: + """Apply a 2D sinusoidal height pattern over x and y. + + The z displacement is formed from an x-varying amplitude and a y-wave: + + ``Az(x) = A * sin(2*pi*x / Tx)`` + ``dz(x, y) = Az(x) * sin(2*pi*y / Ty + phase)`` + + Args: + vertices: Terrain vertices with shape ``(N, 3)``. + A: Maximum z-wave amplitude in meters (float or sampled range). + Tx: Period in meters for amplitude modulation along x. + Ty: Period in meters for sinusoid along y. + N: Number of additive wave passes. + difficulty: Difficulty in ``[0, 1]``. Present for API consistency. + + Returns: + Vertex displacement with shape ``(N, 3)`` where only z is modified. + """ + min_x, max_x = np.min(vertices[:, 0]), np.max(vertices[:, 0]) + length_x = max_x - min_x + x = 2.0 * np.pi * (vertices[:, 0] - min_x) / length_x + xyz = np.zeros_like(vertices) + Az = sample(A) * np.sin(x * round(length_x / sample(Tx))) + num_iter = sample(N) + for _ in range(num_iter): + xyz[:, 2] += Az * np.sin(2.0 * np.pi * vertices[:, 1] / sample(Ty) + sample((0.0, np.pi))) + return xyz + + +def delta_xyz_noise( + vertices: np.ndarray, + U: float | tuple[float, float], + N: int | tuple[int, int], + difficulty: float, +) -> np.ndarray: + """Apply additive uniform noise in all three coordinates. + + Each iteration adds a random offset sampled from ``[-U, +U]`` per + vertex coordinate. + + Args: + vertices: Terrain vertices with shape ``(N, 3)``. + U: Noise magnitude in meters (float or sampled range). + N: Number of additive noise passes. + difficulty: Difficulty in ``[0, 1]``. Present for API consistency. + + Returns: + Vertex displacement with shape ``(N, 3)``. + """ + xyz = np.zeros_like(vertices) + num_iter = sample(N) + for _ in range(num_iter): + xyz += np.random.uniform(-1.0, 1.0, vertices.shape) * sample(U) + return xyz + + +def delta_z_noise( + vertices: np.ndarray, + U: float | tuple[float, float], + N: int | tuple[int, int], + difficulty: float, +) -> np.ndarray: + """Apply additive uniform noise only in z. + + Each iteration adds a random z offset sampled from ``[-U, +U]`` per + vertex. + + Args: + vertices: Terrain vertices with shape ``(N, 3)``. + U: Noise magnitude in meters (float or sampled range). + N: Number of additive noise passes. + difficulty: Difficulty in ``[0, 1]``. Not used by this function. + + Returns: + Vertex displacement with shape ``(N, 3)`` where only z is modified. + """ + xyz = np.zeros_like(vertices) + num_iter = sample(N) + for _ in range(num_iter): + xyz[:, 2] += np.random.uniform(-1.0, 1.0, vertices.shape[0]) * sample(U) + return xyz diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/trail_profiles.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/trail_profiles.py new file mode 100644 index 000000000000..47c7a85163f0 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/trail_profiles.py @@ -0,0 +1,188 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. +from __future__ import annotations + +""" +This module contains functions to generate 2D profiles that represent the +shape of the trail. + +When creating new objects, consider the following: + * Objects are defined in the yz plane. + * The y axis points to the right and the z axis points downward (z + coordinates are flipped). + * The object's anchor point is at (y, z) = (0, 0). + * The object is parameterized as a NumPy array with shape (N, 2). + * The outline of the object is not closed (i.e., the first and last point + are not identical). + + y=0 + +--------+----------------------> y + | | + | | + z=0+--------+ (beginning of object) + | + | + z +""" +import random +from typing import TYPE_CHECKING + +import numpy as np + +from ..utils.math import sample, sample_sign +from . import trail_walls as walls + +if TYPE_CHECKING: + from ..trail_cfg import WallParameters + + +def trail_profile( + width: float, + thickness: float, + wp: WallParameters, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + r"""Helper that generates the trail profile polygon. + + Notes: + * The trail starts at polygon coordinates y=0, z=0. + * The z axis is flipped (points downward). + * The trail is swept along the positive x axis. + + The picture below illustrates the outline polygon with linear walls + and wall_height > 0:: + + y=0 + +--------+-------------------------> y + | +--+ +--+ + | | \ / | + z=0+ | +---------+ | + | | | | | + | | +---------+ | + | +-------------------+ + | + z + + Args: + width: Width of the trail (excluding walls) [m]. + thickness: Thickness of the ground plane [m]. + wp: WallParameters instance used to parameterize the trail walls. + + Returns: + A tuple containing: + * A polygon specifying the profile outline, parameterized as a + NumPy array with shape (N, 2). + * The indices corresponding to the left and right border of the + outline. + * The indices corresponding to all non-trail vertices. + """ + # Dummy border width (used so we can later extend vertices corresponding to the border). + # Do not set this to zero; otherwise trimesh may remove the vertex. + border_width = 0.01 + + # Select the wall signs (+1 means up, -1 means down). + # Sample wall direction from the configured probability distribution. + wall_dir_options = wp.wall_direction + wall_directions = list(wall_dir_options.keys()) + wall_dir_weights = list(wall_dir_options.values()) + wall_direction = random.choices(wall_directions, weights=wall_dir_weights, k=1)[0] + + if wall_direction == "up": + wall_sign = {"left": 1.0, "right": 1.0} + elif wall_direction == "down": + wall_sign = {"left": -1.0, "right": -1.0} + elif wall_direction == "up-down": + s = sample_sign() + wall_sign = {"left": s, "right": -s} + else: + raise RuntimeError(f"Wall direction: '{wall_direction}' is not supported.") + + # Sample wall parameters + def sample_wall(wp: WallParameters) -> tuple[np.ndarray, np.ndarray, bool]: + # Pick a random wall function, weighted by configured probabilities. + wall_options = wp.wall_functions + wall_functions = list(wall_options.keys()) + wall_weights = list(wall_options.values()) + wf_name = random.choices(wall_functions, weights=wall_weights, k=1)[0] + + if not hasattr(walls, wf_name): + raise RuntimeError(f"Unknown wall function '{wf_name}'.") + wf = getattr(walls, wf_name) + + # Compute wall points + return wf( + wall_width=sample(wp.wall_dim["width"]), + wall_height=sample(wp.wall_dim["height"]), + num_segments=sample(wp.num_segments), + ) + + wall_left_yi, wall_left_zi, is_smooth_left = sample_wall(wp=wp) + wall_right_yi, wall_right_zi, is_smooth_right = sample_wall(wp=wp) + + # Decide whether walls are part of the trail segmentation + include_walls_in_trail = { + "left": wall_sign["left"] > 0.0 and is_smooth_left, + "right": wall_sign["right"] > 0.0 and is_smooth_right, + } + + # increase trail thickness if the walls are facing down + if wall_sign["left"] < 0.0 or wall_sign["right"] < 0.0: + thickness += wp.wall_dim["height"] if isinstance(wp.wall_dim["height"], float) else max(wp.wall_dim["height"]) + + # remove the first point of the wall (it is part of the floor already) + wall_left_yi = wall_left_yi[1:] + wall_right_yi = wall_right_yi[1:] + wall_left_zi = wall_left_zi[1:] + wall_right_zi = wall_right_zi[1:] + + # process with sign + wall_right_zi *= wall_sign["right"] + wall_left_zi *= wall_sign["left"] + + # define the knot points of the trail outline + floor_i = np.linspace(0.0, 1.0, sample(wp.num_segments_floor)) + yi = np.concatenate( + [ + floor_i * width, + width + wall_left_yi, + [ + width + wall_left_yi[-1] + border_width, + width + wall_left_yi[-1] + border_width, + -wall_right_yi[-1] - border_width, + -wall_right_yi[-1] - border_width, + ], + -np.flip(wall_right_yi), + ], + axis=0, + ) + + zi = np.concatenate( + [ + floor_i * 0.0, + -wall_left_zi, + [-wall_left_zi[-1], thickness, thickness, -wall_right_zi[-1]], + -np.flip(wall_right_zi), + ], + axis=0, + ) + + # remember border indices + start_id = len(floor_i) + len(wall_left_yi) + border_ids = np.array([start_id, start_id + 1, start_id + 2, start_id + 3]) + + # remember all indices not part of the trail + if include_walls_in_trail["left"]: + non_trail_ids_left = np.array([start_id - 1, start_id, start_id + 1]) + else: + non_trail_ids_left = np.arange(start_id - len(wall_left_yi), start_id + 1) + + if include_walls_in_trail["right"]: + non_trail_ids_right = np.array([start_id + 2, start_id + 3, start_id + 4]) + else: + non_trail_ids_right = np.arange(start_id + 2, start_id + 4 + len(wall_right_yi)) + non_trail_ids = np.concatenate([non_trail_ids_right, non_trail_ids_left], axis=0) + + return (np.stack([yi, zi], axis=1), border_ids, non_trail_ids) diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/trail_walls.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/trail_walls.py new file mode 100644 index 000000000000..6f1362e28676 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/elements/trail_walls.py @@ -0,0 +1,108 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +""" +This module contains functions to generate 2D wall profiles. Walls are used +to separate the trail from the surrounding environment. + +When creating new wall functions, consider the following: + * Walls are defined in the yz plane. + * The y axis points to the right and the z axis points upward. + * The wall's anchor point is at (y, z) = (0, 0). + * The wall is parameterized as a NumPy array with shape (N, 2). + * The function signature for each wall type should be:: + + def function_name(wall_width: float, wall_height: float, num_segments: int): + Args: + wall_width: Width of the wall in the y direction. + wall_height: Height of the wall in the z direction. + num_segments: Number of segments used to parameterize the wall. + + Returns: + y coordinates of the wall with respect to the anchor point, + z coordinates of the wall with respect to the anchor point, + a bool indicating if the wall is smooth (i.e., can be driven onto). + + Example wall shape produced by the functions below:: + + z + ^ + | +----- wall_height + | + | + | + | + +-------+---------+------------> y + y=0 wall_width +""" + +import numpy as np +from scipy.ndimage import gaussian_filter + + +def no_wall(wall_width: float, wall_height: float, num_segments: int) -> tuple[np.ndarray, np.ndarray, bool]: + """Return a tiny, flat wall representation (effectively no wall). + + Uses the minimum number of segments needed to preserve sharp color transitions when the mesh is generated. + """ + num_segments = 2 # minimum number required for a sharp color transition + wall_height = 0.0 + wall_width = 0.1 # keep non-zero to avoid accidental vertex merging + yi = np.linspace(0.0, 1.0, num_segments) + zi = np.linspace(0.0, 1.0, num_segments) + return (yi * wall_width, zi * wall_height, True) + + +def linear_wall(wall_width: float, wall_height: float, num_segments: int) -> tuple[np.ndarray, np.ndarray, bool]: + """Return a linearly sloped wall (constant slope).""" + yi = np.linspace(0.0, 1.0, num_segments) + zi = np.linspace(0.0, 1.0, num_segments) + return (yi * wall_width, zi * wall_height, True) + + +def half_cos_wall(wall_width: float, wall_height: float, num_segments: int) -> tuple[np.ndarray, np.ndarray, bool]: + """Return a wall modeled as a half-cosine that smoothly integrates with the trail.""" + yi = np.linspace(0.0, 1.0, num_segments) + zi = 1.0 - np.cos(0.5 * np.pi * yi) + return (yi * wall_width, zi * wall_height, True) + + +def cos_wall(wall_width: float, wall_height: float, num_segments: int) -> tuple[np.ndarray, np.ndarray, bool]: + """Return a wall modeled as a cosine wave that smoothly integrates with the trail and border.""" + yi = np.linspace(0.0, 1.0, num_segments) + zi = 0.5 * (1.0 - np.cos(np.pi * yi)) + return (yi * wall_width, zi * wall_height, True) + + +def circular_wall(wall_width: float, wall_height: float, num_segments: int) -> tuple[np.ndarray, np.ndarray, bool]: + """Return a wall modeled as a quarter-circle segment.""" + si = np.linspace(0.0, 1.0, num_segments) + yi = np.sin(si * np.pi * 0.5) + zi = 1.0 - np.cos(si * np.pi * 0.5) + return (yi * wall_width, zi * wall_height, True) + + +def gaussian_wall(wall_width: float, wall_height: float, num_segments: int) -> tuple[np.ndarray, np.ndarray, bool]: + """Return a wall with a globally linear slope and local Gaussian roughness. + + The returned wall is not considered "smooth" for driving onto. + """ + # Noise parameters + max_n = 0.2 + sigma = 0.5 + # Baseline linear ramp from trail edge to floor edge. + yi = np.linspace(0.0, 1.0, num_segments) + # Add zero-mean noise before smoothing. + noise = np.random.normal(0.0, max_n, size=num_segments) + zi = yi + noise + # Repeated smoothing produces gentle undulations rather than sharp spikes. + for _ in range(num_segments): + zi = gaussian_filter(zi, sigma=sigma) + # Normalize so the profile starts at 0 and ends at 1. + zi -= zi[0] + if zi[-1] == 0.0: + return gaussian_wall(wall_width, wall_height, num_segments) + zi /= zi[-1] + return (yi * wall_width, zi * wall_height, False) diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/examples/trails.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/examples/trails.py new file mode 100644 index 000000000000..7e547d4cf240 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/examples/trails.py @@ -0,0 +1,309 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +"""Example configuration for custom trail terrains.""" + +from isaaclab.terrains.terrain_generator_cfg import TerrainGeneratorCfg +from isaaclab.utils.configclass import configclass + +from ..elements import roll_functions as roll +from ..trail_cfg import DropCurvesCfg, JumpsCfg, MultipleRampsCfg +from ..trail_cfg import ObjectParameters as OP +from ..trail_cfg import RampsCfg, RootsCfg, SinusoidalCurvesCfg, SkinnyCfg, SlalomCfg, StonesCfg, TerrainParameters +from ..trail_cfg import WallParameters as WP +from ..trail_cfg import WavesCfg, WingCurvesCfg + +# The following proportions were found iteratively. +# If performance is poor on one type, the corresponding weight is increased +TERRAIN_PROPORTIONS = { + "reactive": 6.0, + "drops": 2.5, + "waves": 3.0, + "curves": 7.0, + "skinny": 2.0, + "slalom": 2.5, +} + + +@configclass +class TrailTerrainGeneratorCfg(TerrainGeneratorCfg): + curriculum = False + size = (50.0, 40.0) # length, width + border_width = 0.0 + border_height: float = 0.0 + num_rows = 2 # num terrain levels + num_cols = 4 # num terrain types + difficulty_range = (0.8, 1.0) # 0 for easiest and 1.0 for hardest + use_cache = False # set this to True once you are happy with terrains (speed!!) + + +sub_terrains = {} + +# ---------------------------------------------------------------------------------- +if TERRAIN_PROPORTIONS["reactive"] > 0: + sub_terrains["stones"] = StonesCfg( + proportion=TERRAIN_PROPORTIONS["reactive"] * 0.4, + length_between_objects=(0.0, 0.15), + length_between_platform_and_object=(5.0, 6.0), + cp0=OP(length=(0.2, 0.4), width=2.0, params={"height": 0.01}), + cp1=OP(length=(0.2, 0.8), width=(0.5, 1.5), params={"height": (0.03, 0.08)}), + skip_terrain_functions=["delta_z_noise"], + roll_functions=[roll.sin_x], + roll0=TerrainParameters(params={"Ax": 0.0, "Tx": 25.0}), + roll1=TerrainParameters(params={"Ax": (0.0, 0.15), "Tx": (25.0, 30.0)}), + wp=WP(wall_direction={"up": 1.0}), + ) + sub_terrains["trunks"] = RootsCfg( + proportion=TERRAIN_PROPORTIONS["reactive"] * 0.2, + length_between_objects=(2.0, 6.0), + length_between_platform_and_object=(0.0, 2.0), + cp0=OP( + length=(0.3, 0.5), + width=2.0, + params={ + "height": 0.01, + "amplitude_x": 0.0, + "amplitude_z": 0.0, + "T_xz": (0.5, 2.0), + }, + ), + cp1=OP( + length=(0.3, 0.5), + width=(0.5, 1.5), + params={ + "height": (0.03, 0.2), + "amplitude_x": (0.0, 0.2), + "amplitude_z": (0.0, 0.02), + "T_xz": (0.5, 2.0), + }, + ), + skip_terrain_functions=["delta_z_noise"], + roll_functions=[roll.sin_x], + roll0=TerrainParameters(params={"Ax": 0.0, "Tx": 25.0}), + roll1=TerrainParameters(params={"Ax": (0.0, 0.15), "Tx": (25.0, 30.0)}), + wp=WP(wall_direction={"up": 1.0}), + ) + sub_terrains["roots"] = RootsCfg( + proportion=TERRAIN_PROPORTIONS["reactive"] * 0.4, + length_between_objects=(0.4, 3.0), + length_between_platform_and_object=(0.0, 2.0), + cp0=OP( + length=(0.18, 0.25), + width=2.0, + params={ + "height": 0.01, + "amplitude_x": 0.0, + "amplitude_z": 0.0, + "T_xz": (0.3, 1.5), + }, + ), + cp1=OP( + length=(0.18, 0.25), + width=(0.5, 1.0), + params={ + "height": (0.03, 0.1), + "amplitude_x": (0.0, 0.2), + "amplitude_z": (0.0, 0.03), + "T_xz": (0.3, 1.5), + }, + ), + skip_terrain_functions=["delta_z_noise"], + wp=WP(wall_direction={"up": 1.0}), + ) +# ---------------------------------------------------------------------------------- +if TERRAIN_PROPORTIONS["drops"] > 0: + sub_terrains["drops"] = DropCurvesCfg( + proportion=TERRAIN_PROPORTIONS["drops"] * 0.1, + length_between_objects=(4.0, 7.0), + length_between_platform_and_object=(1.0, 5.0), + cp0=OP(length=(5.5, 6.0), width=2.0, params={"amplitude": 0.0}), + cp1=OP(length=(4.5, 5.0), width=(0.5, 1.5), params={"amplitude": (1.0, 1.9)}), + overhanging_clearance=10.0, + ) + sub_terrains["multiple_ramps"] = MultipleRampsCfg( + proportion=TERRAIN_PROPORTIONS["drops"] * 0.75, + length_between_objects=(4.0, 7.0), + length_between_platform_and_object=(1.0, 5.0), + cp0=OP(length=(1.0, 1.5), width=2.0, params={"height": 0.0}), + cp1=OP(length=(2.0, 4.0), width=(1.8, 2.5), params={"height": (0.4, 0.7)}), + ) + sub_terrains["ramps"] = RampsCfg( + proportion=TERRAIN_PROPORTIONS["drops"] * 0.15, + length_between_objects=(4.0, 7.0), + length_between_platform_and_object=(1.0, 6.0), + cp0=OP(length=(1.0, 1.5), width=2.0, params={"height": 0.0}), + cp1=OP(length=(2.5, 3.0), width=(1.0, 2.0), params={"height": (0.3, 0.7)}), + ) +# ---------------------------------------------------------------------------------- +if TERRAIN_PROPORTIONS["waves"] > 0: + sub_terrains["pits"] = DropCurvesCfg( + proportion=TERRAIN_PROPORTIONS["waves"] * 0.1, + length_between_objects=(0.0, 4.0), + num_curves=2, + cp0=OP(length=(5.5, 6.0), width=2.0, params={"amplitude": 0.0}), + cp1=OP(length=(5.5, 6.0), width=(0.5, 1.5), params={"amplitude": (-0.5, -1.0)}), + overhanging_clearance=10.0, + ) + sub_terrains["big_waves"] = WavesCfg( + proportion=TERRAIN_PROPORTIONS["waves"] * 0.5, + length_between_objects=(0.0, 0.4), + cp0=OP(length=(3.0, 3.5), width=2.0, params={"height": 0.0}), + cp1=OP(length=(3.0, 3.5), width=(1.2, 2.0), params={"height": (0.3, 0.6)}), + roll_functions=[roll.sin_x], + roll0=TerrainParameters(params={"Ax": 0.0, "Tx": 25.0}), + roll1=TerrainParameters(params={"Ax": (0.0, 0.1), "Tx": (25.0, 30.0)}), + ) + sub_terrains["little_waves"] = WavesCfg( + proportion=TERRAIN_PROPORTIONS["waves"] * 0.2, + length_between_objects=(0.0, 0.2), + cp0=OP(length=(1.0, 1.5), width=2.0, params={"height": 0.0}), + cp1=OP(length=(1.0, 1.5), width=(1.2, 2.0), params={"height": (0.05, 0.2)}), + skip_terrain_functions=["delta_z_noise"], + roll_functions=[roll.sin_x], + roll0=TerrainParameters(params={"Ax": 0.0, "Tx": 25.0}), + roll1=TerrainParameters(params={"Ax": (0.0, 0.15), "Tx": (25.0, 30.0)}), + ) + sub_terrains["table_jumps"] = JumpsCfg( + proportion=TERRAIN_PROPORTIONS["waves"] * 0.2, + gap=False, + length_between_objects=(1.0, 2.0), + num_segments=30, + length_between_platform_and_object=(0.5, 2.0), + cp0=OP( + length=(1.5, 2.0), + width=2.0, + params={"height": 0.0, "plateau_proportion": 0.0}, + ), + cp1=OP( + length=(4.0, 5.0), + width=(1.5, 2.0), + params={"height": (0.25, 0.55), "plateau_proportion": (0.35, 0.4)}, + ), + ) +# ---------------------------------------------------------------------------------- +if TERRAIN_PROPORTIONS["curves"] > 0: + sub_terrains["sine_cliff"] = SinusoidalCurvesCfg( + proportion=TERRAIN_PROPORTIONS["curves"] * 0.3, + wp=WP( + wall_functions={"gaussian_wall": 1.0}, + wall_dim={"width": (0.7, 1.1), "height": (0.4, 1.0)}, + wall_direction={"up-down": 0.5, "up": 0.5}, + num_segments=(5, 8), + ), + length_between_objects=(0.0, 1.0), + cp0=OP(length=12.0, width=2.0, params={"amplitude": 0.0}), + cp1=OP(length=(9.0, 12.0), width=(0.5, 1.0), params={"amplitude": (4.0, 6.0)}), + ) + sub_terrains["sine_slalom"] = SinusoidalCurvesCfg( + proportion=TERRAIN_PROPORTIONS["curves"] * 0.2, + length_between_objects=(0.0, 3.0), + cp0=OP(length=15.0, width=2.0, params={"amplitude": 1.5}), + cp1=OP(length=(5.0, 7.0), width=(0.5, 0.7), params={"amplitude": (2.0, 2.5)}), + ) + sub_terrains["wings_double"] = WingCurvesCfg( + proportion=TERRAIN_PROPORTIONS["curves"] * 0.15, + length_between_objects=(0.0, 2.0), + length_between_platform_and_object=(1.0, 2.0), + cp0=OP( + length=10.0, + width=2.0, + params={"radius": 1.0, "rel_angle": 0.0, "slope": 0.0}, + ), + cp1=OP( + length=10.0, + width=(0.5, 1.5), + params={"radius": (0.2, 0.4), "rel_angle": (0.8, 0.9), "slope": (0.0, 0.5)}, + ), + max_wing_length=(6.0, 7.0), + skip_terrain_functions=["delta_z_slope_x"], + ) + sub_terrains["wings_single"] = WingCurvesCfg( + proportion=TERRAIN_PROPORTIONS["curves"] * 0.1, + length_between_objects=100.0, + length_between_platform_and_object=(5.0, 8.0), + cp0=OP( + length=10.0, + width=2.0, + params={"radius": 1.0, "rel_angle": 0.0, "slope": 0.0}, + ), + cp1=OP( + length=10.0, + width=(0.5, 2.0), + params={ + "radius": (0.1, 0.4), + "rel_angle": (0.9, 0.95), + "slope": (0.0, 0.4), + }, + ), + max_wing_length=9.0, + ) + sub_terrains["wings_slalom"] = WingCurvesCfg( + proportion=TERRAIN_PROPORTIONS["curves"] * 0.25, + length_between_objects=(0.0, 2.0), + cp0=OP( + length=15.0, + width=2.0, + params={"radius": 2.0, "rel_angle": 0.0, "slope": 0.0}, + ), + cp1=OP( + length=(5.0, 6.0), + width=(0.5, 0.7), + params={"radius": 0.2, "rel_angle": (0.6, 0.7), "slope": (-0.15, 0.15)}, + ), + max_wing_length=8.0, + ) +# ---------------------------------------------------------------------------------- +if TERRAIN_PROPORTIONS["skinny"] > 0: + sub_terrains["skinny_grounded"] = SkinnyCfg( + proportion=TERRAIN_PROPORTIONS["skinny"], + length_between_objects=(3.0, 5.0), + length_between_platform_and_object=(0.0, 1.0), + cp0=OP( + length=2.0, + width=2.5, + params={ + "beam_thickness": 0.0, + "amplitude_y": 0.0, + "amplitude_z": 0.0, + "rel_knot_point": (0.25, 0.4), + "beam_width": 0.7, + }, + ), + cp1=OP( + length=(3.0, 7.0), + width=(1.5, 2.5), + params={ + "beam_thickness": (0.1, 0.15), + "amplitude_y": (0.0, 0.2), + "amplitude_z": (0.0, 0.1), + "rel_knot_point": (0.25, 0.4), + "beam_width": (0.4, 0.7), + }, + ), + trail_under_object=None, + ) +# ---------------------------------------------------------------------------------- +if TERRAIN_PROPORTIONS["slalom"] > 0: + sub_terrains["slalom"] = SlalomCfg( + proportion=TERRAIN_PROPORTIONS["slalom"], + length_between_objects=(1.0, 3.5), + length_between_platform_and_object=(1.0, 3.0), + cp0=OP( + length=0.4, + width=2.0, + params={"height": 1.0, "rel_dist_from_center": (1.0, 1.0)}, + ), + cp1=OP( + length=(0.25, 0.4), + width=(1.1, 2.0), + params={"height": (0.2, 1.0), "rel_dist_from_center": (0.5, 1.0)}, + ), + roll1=TerrainParameters(params={"A": (0.0, 0.2)}), + ) + +# ---------------------------------------------------------------------------------- +TRAIL_CFG = TrailTerrainGeneratorCfg(sub_terrains=sub_terrains) diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/terrain_parameter_interface.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/terrain_parameter_interface.py new file mode 100644 index 000000000000..8f7bec69655c --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/terrain_parameter_interface.py @@ -0,0 +1,59 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +from isaaclab.utils.configclass import configclass + +from .trail_cfg import TrailBaseCfg + + +@configclass +class TerrainParameterInterface: + """This class exposes terrain parameters shared between trail terrain generators and MDP components. + + It also documents the parametrization scheme used to construct a terrain + patch, illustrated below:: + + | -B- |==P0==|========TRAIL==========|==P1==| -B- | + + +------------------------------------------------+----> x + 0 size[0] + + where: + B: border of width ``border_width``. + P0: start platform of length ``platform_length``. + P1: end platform of length ``platform_length``. + TRAIL: trail path connecting the initial and final platforms. + size: (length, width) of the terrain patch. + """ + + def __init__(self, source_cfg: TrailBaseCfg = TrailBaseCfg()): + """Constructor. + + Args: + source_cfg: The source configuration from which parameters are copied. + """ + self.border_width = source_cfg.border_width + self.platform_length = source_cfg.platform_length + self.distance_start_to_trail = source_cfg.distance_start_to_trail + + def get_trail_length(self, size: tuple[float, float]) -> float: + """Absolute distance between P0 and P1 along the x axis in world frame [m].""" + return size[0] - 2.0 * (self.platform_length + self.border_width) + + def get_distance_init_to_center(self, size: tuple[float, float]) -> float: + """Distance from the spawn location to the terrain patch center along x [m].""" + return self.distance_start_to_trail + 0.5 * self.get_trail_length(size=size) + + def get_center_to_terrain_border(self, size: tuple[float, float]) -> tuple[float, float]: + """Return distances (dx, dy) from the terrain center to its borders. + + Returns distances along x and y from the patch center to the inner border (i.e., excluding the outer border + width). + """ + dim_x = 0.5 * size[0] - self.border_width + dim_y = 0.5 * size[1] - self.border_width + return (dim_x, dim_y) diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/trail_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/trail_cfg.py new file mode 100644 index 000000000000..614424a6f41d --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/trail_cfg.py @@ -0,0 +1,862 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +from __future__ import annotations + +from collections.abc import Callable +from functools import partial +from typing import Literal + +import numpy as np +import trimesh +from shapely.geometry import Polygon + +from isaaclab.terrains.terrain_generator_cfg import SubTerrainBaseCfg +from isaaclab.utils.configclass import configclass + +from . import trail_terrains +from .elements import decoration_functions as decoration +from .elements import object_profiles as profiles +from .elements import roll_functions as roll +from .elements import sweeping_paths as paths +from .elements import terrain_functions as terrain +from .utils import colors, transformations +from .utils.math import sample, sample_sign +from .utils.numpy_arrays import mirror_and_join + + +@configclass +class ObjectParameters: + """Parameters in this class are used to parametrize the trail objects, and are varied linearly depending on the + current terrain level.""" + + length: float | tuple[float, float] = 1.0 + """The length of each object, measured in heading direction of the trail. + + [m] + """ + + width: float | tuple[float, float] = 1.0 + """The width of the trail. + + [m] + """ + + params: dict[str, float | tuple[float, float]] = {} + """Collection of optional parameters which are used to parametrize the trail objects.""" + + +@configclass +class TerrainParameters: + """Parameters in this class are used to parametrize the trail terrain, and are varied linearly depending on the + current terrain level.""" + + params: dict[str, float | tuple[float, float]] = {} + """Collection of optional parameters which are used to parametrize the trail terrain.""" + + +@configclass +class WallParameters: + """Parameters in this class are used to parametrize the walls of terrain. + + Parameters do not change as a function of the terrain level. + """ + + wall_functions: dict[str, float] = { + "no_wall": 0.2, + "linear_wall": 0.5, + "half_cos_wall": 1.0, + "cos_wall": 1.5, + "circular_wall": 0.0, + "gaussian_wall": 0.5, + } + """Mapping from wall-shape function name to sampling probability weight. + + A wall function is sampled randomly according to its weight. + """ + + wall_dim: dict[str, float | tuple[float, float]] = { + "width": (0.2, 0.7), + "height": (0.1, 0.4), + } + """The wall dimensions are specified by these values [m].""" + + wall_direction: dict[str, float] = {"up": 1.0, "down": 0.1, "up-down": 0.1} + """Mapping from wall direction to sampling probability weight. + + Supported directions are: "up", "down", "up-down". + A direction is sampled randomly according to its weight. + """ + + num_segments: int | tuple[int, int] = 8 + """Number of segments used to define each side of the walls. + + Default is 8. + """ + + num_segments_floor: int = 8 + """Number of segments used to define the ground floor. + + Default is 4. The minimum is 1, indicating a linear evolution of the ground along the lateral direction of the + trail. + """ + + @property + def max_wall_width(self) -> float: + """Helper function that returns an upper estimate of the trail width.""" + return self.wall_dim["width"][1] if isinstance(self.wall_dim["width"], tuple) else self.wall_dim["width"] + + +@configclass +class ColorParameters: + """Parameters in this class are used to color the trail environment. + + Colors are defined in HSV convention and per object/segment. + """ + + color_mesh: bool = True + """If true, the mesh is colored according to the color plate specified below.""" + + hsv: dict[str, float | tuple[float, float]] = colors.TRAIL + """HSV (or rgb) color range.""" + + uniform: bool = False + """If true, the object/segment will be colored uniformly.""" + + +@configclass +class TrailBaseCfg(SubTerrainBaseCfg): + function = trail_terrains.mesh_trail_segment + """Generic function to generate the trail terrains. + + This function does not need to be changed nor configured. Find all configuration parameters below. + """ + # --- Surroundings --- + # The following parameters describe the local surroundings of the trail. + platform_length: float = 10.0 + """Length of the starting and ending platform [m].""" + thickness: float = 1.0 + """The thickness of the ground [m]. + + Make sure this value is not too small, otherwise artifacts may arise as vertices below the ground are extended + beyond the surface. + """ + border_width: float = 2.0 + """Border added to both sides of the terrain patch. + + This is useful for visual separation of the terrain patches [m]. + """ + ride_direction: Literal["downhill", "uphill"] = "downhill" + """The direction in which the trail is traversed. + + Can be downhill (default), or uphill. + """ + floor_width: float | None = None + """Width of the floor added to the left and the right of the trail. + + If larger than the terrain width, the values are clipped. None means the floor is extended to the borders. Default + is None. + """ + path_res: float = 0.5 + """Any two adjacent points on the trail path are at least this far away. + + [m] + """ + wp: WallParameters = WallParameters() + """Parameters defining the walls, i.e., the connection between the trail and the floor.""" + # --- Trail objects --- + # Trail objects are objects that can be typically encountered on a trail or pump track, such as waves or drops. + object_function: Callable[[float, float, dict[str, float], TrailBaseCfg], np.ndarray | trimesh.Trimesh] | None = ( + None + ) + """Function that defines objects placed on the trail. + + It returns one of the following: * ``NumPy array``: a 2D profile which is swept into a 3D mesh + * ``trimesh.Trimesh``: a 3D mesh which does not require a sweeping path + * ``None``: indicating that no objects are created along the trail path. + """ + extrude_trail_objects: bool = False + """If False, trail objects are represented as additional meshes merged with the trail mesh. + + If True, the trail mesh is re-meshed to approximate the objects. This can reduce the total number of vertices when + an object is large but contains relatively few vertices. + """ + sweeping_path: Callable[[float, float, dict[str, float], TrailBaseCfg], np.ndarray] | None = None + """Function that defines how 2D objects are swept into 3D objects. + + This function is only used if object_function is not None. + """ + object_transformation: Callable[[float, float, dict[str, float], TrailBaseCfg], np.ndarray] | None = None + """This function can be used to define a certain pose with respect to the trail path. + + ``None`` means identity. + """ + trail_under_object: Callable[[float, float, dict[str, float], TrailBaseCfg], np.ndarray] | None = None + """This sweeping function can be used to define the trail under the object. + + None means the local terrain is flat. + """ + num_segments: int = 8 + """Curved objects are approximated by this many segments. + + Depending on the object, this number may have a different meaning. + """ + length_between_objects: float | tuple[float, float] = 0.0 + """Spacing between any two objects in x direction, measured from end to start [m].""" + length_between_platform_and_object: float | tuple[float, float] = 0.5 + """Spacing between an object and a platform in x direction, measured from end to start [m].""" + cp0: ObjectParameters = ObjectParameters() + """Object parameters at curriculum start.""" + cp1: ObjectParameters = ObjectParameters() + """Object parameters at curriculum end.""" + # --- Trail shape --- + # The trail shape defines the overall curvature and inclination of the trail. + terrain_functions: list[Callable[[np.ndarray, dict[str, float]], np.ndarray]] | None = [ + partial(terrain.delta_i_sin_x, dim=0, A=(0.0, 0.1), T=(6.0, 16.0), N=(1, 2)), + partial(terrain.delta_i_sin_x, dim=1, A=(0.1, 0.3), T=(8.0, 16.0), N=(1, 2)), + partial(terrain.delta_i_sin_x, dim=2, A=(0.1, 0.1), T=(6.0, 16.0), N=(1, 2)), + partial(terrain.delta_z_sin_xy, A=(0.0, 0.1), Tx=(6.0, 30.0), Ty=(2.0, 6.0), N=1), + partial(terrain.delta_z_slope_x, S=(0.0, 0.15)), + partial(terrain.delta_z_noise, U=(0.0, 0.02), N=1), + ] + """Set of functions that specify the raw terrain. + + The functions are applied in the order that they are added to the list. The parameters for these functions are place + in TerrainParameters. If set to None, the terrain is flat. + """ + skip_terrain_functions: list[str] | None = None + """List of terrain functions to skip. + + This can be used to disable certain terrain functions without changing the curriculum parameters. + """ + # --- Trail roll --- + # Trail roll is an additional DOF of the trail shape. + roll_functions: list[Callable[[np.ndarray, dict[str, float]], np.ndarray]] | None = [roll.const] + """Set of functions that specify the roll (i.e., the angle around the heading direction). + + The functions are applied in the order that they are added to the list. If set to None, the terrain is flat. + """ + roll0: TerrainParameters = TerrainParameters(params={"A": 0.0}) + """Roll parameters at curriculum start.""" + roll1: TerrainParameters = TerrainParameters(params={"A": (0.0, 0.1)}) + """Roll parameters at curriculum end.""" + # --- Decorations --- + # decorative elements are trees, stones, roots, etc, which are placed next to the trail. + distance_between_decorations: float | tuple[float, float] = (1.5, 5.0) + """Distance between any two decorative objects, sampled per terrain patch. + + [m] + """ + trail_border_clearance: float | tuple[float, float] = (-0.1, 0.2) + """Distance between the trail border and the center of the decoration object. + + [m] + """ + decoration_functions: list[Callable[[dict(str, list[trimesh.Trimesh]), TrailBaseCfg], trimesh.Trimesh]] | None = [ + partial( + decoration.load_object_mesh, + object_dist={"evergreen": 0.90, "roots": 0.05, "rocks": 0.05}, + ), + partial( + decoration.load_object_mesh, + object_dist={"summer": 0.9, "roots": 0.05, "rocks": 0.05}, + ), + partial( + decoration.load_object_mesh, + object_dist={"evergreen": 0.2, "winter": 0.7, "roots": 0.05, "rocks": 0.05}, + ), + partial( + decoration.load_object_mesh, + object_dist={ + "evergreen": 0.45, + "summer": 0.45, + "roots": 0.05, + "rocks": 0.05, + }, + ), + partial( + decoration.load_object_mesh, + object_dist={"evergreen": 0.05, "rocks": 0.95}, + ), + ] + """List of functions used to generate decorative objects. + + Sampled per terrain patch according to + ``decoration_function_weights``. If ``None``, no decorative elements are placed next to the trail. + """ + decoration_function_weights: list[float] | None = [4.0, 1.0, 1.0, 1.0, 1.0] + """Sampling weights for ``decoration_functions``. + + Must have the same length. If ``None``, uniform weights are used. + """ + cut_objects_above: float | None = 2.0 + """Decorative objects are cut above this distance to reduce the number of vertices. + + If ``None``, no cutting is performed. [m] + """ + convex_approx: bool = False + """If true, decoration objects are approximated using convex hull to reduce the total number of vertices.""" + rel_decorated_terrains: float = 1.0 + """If decorative objects are used, this fraction of terrain patches will include them. + + Must be a value in [0, 1]. + """ + num_decoration_layers: int = 1 + """Number of layers of added decorative objects. + + The first layer is left/right to the trail, the second layer extends the first layer, etc. + """ + overhanging_clearance: float = 2.5 + """The trail is guaranteed to have no overhanging objects below this distance. + + [m] + """ + # --- Coloring --- + col_trail = ColorParameters(hsv=colors.TRAIL) + col_trail_object = ColorParameters(hsv=colors.TRAIL) + col_trail_under_object = ColorParameters(hsv=colors.TRAIL) + col_floor = ColorParameters(hsv=colors.FLOOR) + col_start = ColorParameters(hsv=colors.START) + col_goal = ColorParameters(hsv=colors.GOAL) + # --- Others ---- + training: bool = True + """Indicates if terrain is used for training or sim deployment. + + Depending on this flag, objects will be more or less simplified. + """ + distance_start_to_trail: float = 2.5 + """The starting location of the robots is this far away from the trail start. + + [m] + """ + + +@configclass +class WavesCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of waves. + + * Each wave consists of a ramp up and ramp down. + * The curvature of the ramps depends on its length and height. + """ + + def object_function(length: float, width: float, params: dict[str, float], cfg: WavesCfg) -> np.ndarray: + """Create the 2D profile for a wave object. + + Args: + length: total object length along the trail [m]. + width: lateral width of the trail [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + A NumPy array describing the 2D profile to be swept into 3D. + """ + return profiles.wave_profile( + length=0.5 * length, + height=params["height"], + num_segments=cfg.num_segments, + platform_length=0.0, + gap=False, + type="cos", + ) + + def sweeping_path(length: float, width: float, params: dict[str, float], cfg: WavesCfg) -> np.ndarray: + """Provide the sweeping path used to extrude a 2D profile into 3D. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled object parameters. + cfg: configuration instance for this terrain. + + Returns: + Path array used for sweeping the profile. + """ + return paths.linear_path_y(width=width, num_segments=cfg.wp.num_segments_floor) + + # overwrite + extrude_trail_objects = True + num_segments = 14 + + +@configclass +class JumpsCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of jumps. + + * Each jump consists of a ramp up, a plateau platform, and ramp down. + * The curvature of the jump depends on its length, height and the plateau length. + """ + + def object_function(length: float, width: float, params: dict[str, float], cfg: WavesCfg) -> np.ndarray: + """Create the 2D profile for a jump object. + + Args: + length: total object length along the trail [m]. + width: lateral width of the trail [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + A NumPy array describing the 2D profile to be swept into 3D. + """ + plateau_length = sample(params["plateau_proportion"]) * length + ramp_length = 0.5 * (length - plateau_length) + return profiles.wave_profile( + length=ramp_length, + height=params["height"], + num_segments=cfg.num_segments, + platform_length=plateau_length, + gap=cfg.gap, + exponent=cfg.exponent, + type="half-cos", + ) + + def sweeping_path(length: float, width: float, params: dict[str, float], cfg: WavesCfg) -> np.ndarray: + """Provide the sweeping path for jump objects. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled object parameters. + cfg: configuration instance for this terrain. + + Returns: + Path array used for sweeping the profile. + """ + return paths.linear_path_y(width=width, num_segments=cfg.wp.num_segments_floor) + + gap: bool | tuple[bool, bool] = True + """If true, the plateau is embedded into the ground, forming a gap between the two ramps. + + The gap is parametrized by a cosine shaped profile. + """ + exponent: int | tuple[int, int] = (4, 10) + """The larger the exponent is chosen, the sharper the gap becomes. + + Only used if gap is True. + """ + # overwrite + extrude_trail_objects = True + num_segments = 50 + + +@configclass +class StonesCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through patches of gravel roads.""" + + def object_function(length: float, width: float, params: dict[str, float], cfg: StonesCfg) -> np.ndarray: + """Create the 2D profile for a stone/rock object. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + A NumPy array describing the 2D profile to be swept into 3D. + """ + return profiles.root_profile( + length=length, + height=params["height"], + num_segments=cfg.num_segments, + exponent=cfg.exponent, + ) + + def sweeping_path(length: float, width: float, params: dict[str, float], cfg: StonesCfg) -> np.ndarray: + """Provide the sweeping path for stone objects. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + Path array used for sweeping the profile. + """ + return paths.sinusoidal_exp_z_curve_y( + width=width, + stone_length=length, + stone_height=params["height"], + distance_between_stones=cfg.length_between_objects, + exponent=cfg.exponent, + num_segments=cfg.num_segments, + ) + + exponent: int | tuple[int, int] = (1, 5) + """The larger the exponent is chosen, the more the stone profile approaches a square. + + A value of 1 means a sinusoidal shape. + """ + # color trail objects uniformly (overwrite) + col_trail_object = ColorParameters(hsv=colors.TRAIL, uniform=True) + + +@configclass +class RootsCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a road of roots.""" + + def object_function(length: float, width: float, params: dict[str, float], cfg: RootsCfg) -> np.ndarray: + """Create the 2D profile for a root object. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + A NumPy array describing the 2D profile to be swept into 3D. + """ + return profiles.root_profile( + length=length, + height=params["height"], + num_segments=cfg.num_segments, + exponent=cfg.exponent, + ) + + def sweeping_path(length: float, width: float, params: dict[str, float], cfg: RootsCfg) -> np.ndarray: + """Provide the sweeping path for root objects. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + Path array used for sweeping the profile. + """ + total_width = cfg.wp.max_wall_width + width + return paths.sinusoidal_xz_curve_y( + width=2.0 * cfg.wp.max_wall_width + width, + amplitude_x=params["amplitude_x"], + amplitude_z=params["amplitude_z"], + T_xz=params["T_xz"], + num_segments=int(total_width * cfg.num_segments * 2.0), + ) + + exponent: int | tuple[int, int] = (1, 4) + """The larger the exponent is chosen, the more the root profile approaches a square. + + A value of 1 means a sinusoidal shape. + """ + # color trail objects uniformly (overwrite) + col_trail_object = ColorParameters(hsv=colors.TRAIL, uniform=True) + + +@configclass +class RampsCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of ramps. + + * The height specifies the height above ground for the drop. + """ + + def object_function(length: float, width: float, params: dict[str, float], cfg: RampsCfg) -> np.ndarray: + """Create the 2D profile for a ramp object. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + A NumPy array describing the 2D profile to be swept into 3D. + """ + return profiles.ramp_profile( + length=length, + height=params["height"], + num_segments=cfg.num_segments, + elevation=sample(cfg.box_height), + ) + + def sweeping_path(length: float, width: float, params: dict[str, float], cfg: RampsCfg) -> np.ndarray: + """Provide the sweeping path for ramp objects. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + Path array used for sweeping the profile. + """ + return paths.linear_path_y( + width=sample(cfg.rel_ramp_width) * width, + num_segments=cfg.wp.num_segments_floor, + ) + + rel_ramp_width: float | tuple[float, float] = (0.7, 0.9) + """Relative width of the ramp with respect to the trail width.""" + box_height: float | tuple[float, float] = (0.0, 0.1) + """The slope is placed on a box of this height.""" + + +@configclass +class MultipleRampsCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of parallel ramps. + + * The height specifies the height above ground for the drop. + """ + + def object_function( + length: float, width: float, params: dict[str, float], cfg: MultipleRampsCfg + ) -> trimesh.Trimesh: + num_ramps = sample(cfg.num_ramps) + lane_width = width / num_ramps + ramps = [] + + rel_heights = np.array([sample(cfg.rel_height) for _ in range(num_ramps)], dtype=float) + max_idx = int(np.argmax(rel_heights)) + rel_heights[max_idx] = 1.0 # largest ramp + + for i in range(num_ramps): + ramp_height = rel_heights[i] * params["height"] + ramp_profile = profiles.ramp_profile( + length=length, + height=ramp_height, + num_segments=cfg.num_segments, + elevation=sample(cfg.box_height), + ) + sweeping_path = paths.linear_path_y(width=lane_width, num_segments=cfg.wp.num_segments_floor) + sweeping_path[:, 1] += -0.5 * width + (i + 0.5) * lane_width + ramps.append( + trimesh.creation.sweep_polygon( + Polygon(ramp_profile), + path=sweeping_path, + engine=trail_terrains.ENGINE, + ) + ) + + return ramps[0] if len(ramps) == 1 else trimesh.util.concatenate(ramps) + + def object_transformation(length: float, width: float, params: dict[str, float], cfg: RampsCfg) -> np.ndarray: + return transformations.translation(vec=[0.0, 0.0, 0.0]) + + rel_height: float | tuple[float, float] = (0.5, 1.0) + """Relative height scaling for each ramp with respect to height.""" + num_ramps: int | tuple[int, int] = (2, 3) + """Number of parallel ramps placed across the trail width.""" + box_height: float | tuple[float, float] = (0.0, 0.1) + """The slope is placed on a box of this height.""" + + +@configclass +class SinusoidalCurvesCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of sinusoidal curves + in xy plane. + + * The length specifies the curve length in x direction of the trail. + * The sign of the amplitude is randomly sampled. + """ + + def trail_under_object( + length: float, width: float, params: dict[str, float], cfg: SinusoidalCurvesCfg + ) -> np.ndarray: + """Provide a local trail profile beneath objects for sinusoidal-curve terrains. + + Args: + length: object length [m]. + width: trail width [m]. + params: sampled parameters for the object. + cfg: configuration instance for this terrain. + + Returns: + Path array representing the trail under the object. + """ + return paths.sinusoidal_y_curve_x( + length=length, + amplitude=sample_sign() * params["amplitude"], + num_curves=cfg.num_curves, + num_segments=cfg.num_segments, + ) + + num_curves: int | tuple[int, int] = 1 + """Number of sinusoidal curves put in sequence. + + 1 means sinusoidal curve in (0,2*pi). + """ + # overwrite + num_segments = 20 + + +@configclass +class DropCurvesCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of sinusoidal curves + in xz plane. + + * The length specifies the curve length in x direction of the trail. + * The sign is set by the sign of height. Positive value means wave up. + """ + + def trail_under_object(length: float, width: float, params: dict[str, float], cfg: DropCurvesCfg) -> np.ndarray: + return paths.sinusoidal_z_curve_x( + length=length, + amplitude=params["amplitude"], + num_curves=cfg.num_curves, + num_segments=cfg.num_segments, + ) + + num_curves: int | tuple[int, int] = 1 + """Number of sinusoidal curves put in sequence. + + 1 means sinusoidal curve in (0,pi). + """ + + +@configclass +class WingCurvesCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of three circular + curves in xy plane. + + * The length specifies the curve length in x direction of the trail. Attention: the requested length may be smaller. + * The direction of the curve is randomly chosen. + * Construction of the curve is only possible if (0.5*length - 2*height > 0) and (2*height>max_wing_length). + """ + + def trail_under_object(length: float, width: float, params: dict[str, float], cfg: WingCurvesCfg) -> np.ndarray: + return paths.circular_xy_curve_x( + length=length, + radius=params["radius"] + cfg.wp.max_wall_width + 0.5 * width, + rel_angle=sample_sign() * params["rel_angle"], + slope=params["slope"], + max_wing_length=cfg.max_wing_length, + num_segments=cfg.num_segments, + ) + + max_wing_length: float | tuple[float, float] = 10.0 + """Largest allowed overshoot in y direction of the trail, measured from center to the most outer part of the wing. + + Make sure to choose this value s.t. the curve does not penetrate into neighboring terrains. + """ + + +@configclass +class SkinnyCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of bridges. + + * Each bridge is modeled as a beam, which has two turns. + * The bridge is spawned above a sinusoidal valley. + """ + + def object_function(length: float, width: float, params: dict[str, float], cfg: SkinnyCfg) -> np.ndarray: + return profiles.box_profile(length=params["beam_width"], height=params["beam_thickness"]) + + def sweeping_path(length: float, width: float, params: dict[str, float], cfg: SkinnyCfg) -> np.ndarray: + return paths.zig_zag_yz_path_x( + length=length, + amplitude_y=params["amplitude_y"], + amplitude_z=params["amplitude_z"], + rel_knot_point=params["rel_knot_point"], + ) + + def object_transformation(length: float, width: float, params: dict[str, float], cfg: SkinnyCfg) -> np.ndarray: + offset_from_center_y = sample((-0.5, 0.5)) * abs(width - params["beam_width"]) + vec = [ + 0.0, + 0.5 * params["beam_width"] + offset_from_center_y, + cfg.rel_distance_beam_above_ground * params["beam_thickness"], + ] + return transformations.translation(vec=vec) + + def trail_under_object(length: float, width: float, params: dict[str, float], cfg: SkinnyCfg) -> np.ndarray: + rel_flat_distance = sample(cfg.rel_flat_distance) + curve = paths.sinusoidal_z_curve_x( + length=length * (1.0 - rel_flat_distance) * 0.5, + amplitude=-params["height"], + num_curves=1, + num_segments=cfg.num_segments, + ) + return mirror_and_join(object=curve, dim=0, offset=length * rel_flat_distance) + + rel_flat_distance: float | tuple[float, float] = (0.0, 0.4) + """This parameter is useful to alter the curvature of the terrain underneath the beam. + + 1 indicates 90 degrees edges, 0 indicates a pure cosine wave. + """ + rel_distance_beam_above_ground: float = 0.0 + """Relative distance of the beam above the ground with respect to the beam thickness. + + For example, 0.5 means the beam is placed half of its thickness above the ground. + """ + + +@configclass +class Loops360Cfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of loops in the xz + plane.""" + + def object_function(length: float, width: float, params: dict[str, float], cfg: Loops360Cfg) -> np.ndarray: + return profiles.box_profile(length=cfg.loop_thickness, height=params["loop_width"]) + + def sweeping_path(length: float, width: float, params: dict[str, float], cfg: Loops360Cfg) -> np.ndarray: + return paths.loop_curve_x( + displacement_z=sample_sign() * params["loop_width"], + radius=0.5 * length, + angle=2.0 * np.pi, + num_segments=cfg.num_segments, + ) + + def object_transformation(length: float, width: float, params: dict[str, float], cfg: Loops360Cfg) -> np.ndarray: + vec = [0.0, 0.5 * params["loop_width"], -cfg.loop_thickness] + return transformations.translate_and_roll(vec=vec, angle=0.5 * np.pi) + + loop_thickness: float = 0.2 + """The thickness of the loop [m].""" + # color trail objects uniformly (overwrite) + col_trail_object = ColorParameters(hsv=colors.TRAIL, uniform=True) + + +@configclass +class StairsCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end, which are connected through a sequence of stair cases.""" + + def object_function(length: float, width: float, params: dict[str, float], cfg: StairsCfg) -> np.ndarray: + return profiles.stair_profile(length=length, height=params["height"], step_width=params["step_width"]) + + def sweeping_path(length: float, width: float, params: dict[str, float], cfg: StairsCfg) -> np.ndarray: + return paths.linear_path_y(width=width, num_segments=cfg.wp.num_segments_floor) + + def trail_under_object(length: float, width: float, params: dict[str, float], cfg: StairsCfg) -> np.ndarray: + return np.vstack([[0.0, 0.0, 0.0], [length, 0.0, params["height"]]]) + + +@configclass +class SlalomCfg(TrailBaseCfg): + """This terrain consists of two platforms on each end. + + The trail contains cylinders standing upright, forming a slalom like course. + """ + + def object_function(length: float, width: float, params: dict[str, float], cfg: SlalomCfg) -> trimesh.Trimesh: + # create object + object = trimesh.creation.cylinder(radius=length, height=1.2 * params["height"], sections=sample(cfg.num_edges)) + # add noise to randomize the pole + delta_xy = cfg.noise_level * length + delta_h = cfg.noise_level * params["height"] + object.vertices[:, 0] += np.random.uniform(-delta_xy, delta_xy, size=object.vertices.shape[0]) + object.vertices[:, 1] += np.random.uniform(-delta_xy, delta_xy, size=object.vertices.shape[0]) + object.vertices[:, 2] += np.random.uniform(-delta_h, delta_h, size=object.vertices.shape[0]) + return object + + def object_transformation(length: float, width: float, params: dict[str, float], cfg: SlalomCfg) -> np.ndarray: + delta_y = (width + cfg.wp.max_wall_width) * sample(params["rel_dist_from_center"]) * sample_sign() + return transformations.translation(vec=[0.0, 0.5 * delta_y, 0.4 * params["height"]]) + + # redefine the pillar color to be part of the floor, and color it uniformly (overwrite) + col_trail_object = ColorParameters(hsv=colors.FLOOR, uniform=True) + + num_edges: int | tuple[int, int] = (4, 8) + """Number of edges of the pose.""" + noise_level: float = 0.2 + """The noise level added to the pole geometry to randomize the shape. + + This is a fraction of the length and height of the pole. + """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/trail_terrains.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/trail_terrains.py new file mode 100644 index 000000000000..02c99eef6dcb --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/trail_terrains.py @@ -0,0 +1,762 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +from __future__ import annotations + +import os.path +import random +from collections import defaultdict +from typing import TYPE_CHECKING, Literal + +# python +import numpy as np +import trimesh +from shapely import minimum_bounding_circle, minimum_bounding_radius +from shapely.geometry import MultiPoint, Polygon + +from .elements import decoration_functions, trail_profiles +from .utils import colors, transformations, trimesh_utils +from .utils.math import interp, interp_dict_and_sample, sample +from .utils.numpy_arrays import get_bounding_box + +if TYPE_CHECKING: + from . import trail_cfg + +# global parameters +ENGINE: str = "manifold" # geometry library (e.g. manifold, scad, blender) +EPS: float = 0.01 # smallest mesh resolution in m + + +def mesh_trail_segment( + difficulty: float, + cfg: trail_cfg.TrailBaseCfg, +) -> tuple[trimesh.Trimesh, np.ndarray]: + """Helper function to create a mesh for a trail segment with similar objects. + + The trail is constructed as follows + + y + ^ trail direction + | start -----------> end + | + | sweeping direction + | P0 ------------> P1 + | + | +----+---------------------------+----+ + y=0 +-- | P0 | trail with objects/curves | P1 | --- y=0 --- + | +----+---------------------------+----+ + | + +-----+----------------------------------------------------> x + x=0 + + Some notes: + * P0 indicates the starting platform and P1 the ending platform. + * P0 and P1 are always aligned with and centered at the y-axis, even if the trail contains curves. + * The trail is constructed along positive x axis, i.e., from P0 to P1. + * The trail is constructed by sweeping an outline polygon along the x-axis in the positive direction, + i.e., from P0 to P1. + + Args: + difficulty: The difficulty parameter, 0 for easy and 1 for hard. + cfg: The configuration of the sub-terrain. + + Returns: + Mesh of the terrain and its origin with respect to world-frame coordinates. + """ + # interpolate curriculum parameters + width_limits = interp(param0=cfg.cp0.width, param1=cfg.cp1.width, x=difficulty) + length_limits = interp(param0=cfg.cp0.length, param1=cfg.cp1.length, x=difficulty) + + # sample segment parameters + length_between_platform_and_object = sample(cfg.length_between_platform_and_object) + width = sample(width_limits) + + # initialize mesh and trail path + mesh = trimesh.Trimesh() + object_meshes: list[tuple[trimesh.Trimesh, float]] = [] + trail_path = np.array([[0.0, 0.0, 0.0]]) + object_location = np.array([length_between_platform_and_object, 0.0, 0.0]) + trail_x_under_object = None + + # bounding box of trail + max_trail_size: tuple[float, float] = [ + cfg.size[0] - 2.0 * cfg.platform_length - 2.0 * cfg.border_width, + cfg.size[1] - 2.0 * cfg.border_width, + ] + + # generate sequence of objects, starting from platform P0 + while True: + # sample object parameters + object_length = sample(length_limits) + length_between_objects = max(sample(cfg.length_between_objects), EPS) + object_values = interp_dict_and_sample(cp0=cfg.cp0, cp1=cfg.cp1, x=difficulty) + + # ensure that the object fits onto the trail + if ( + difficulty <= 0.05 + or object_location[0] + object_length + length_between_platform_and_object > max_trail_size[0] + ): + object_location[0] = max_trail_size[0] # move to the end + trail_path = np.vstack([trail_path, object_location]) + break + + # get profile polygon and sweeping path for a single object + object = None + path = None + if cfg.object_function is not None: + object = cfg.object_function(length=object_length, width=width, params=object_values, cfg=cfg) + if cfg.sweeping_path is not None: + path = cfg.sweeping_path(length=object_length, width=width, params=object_values, cfg=cfg) + + # add trail object + if object is not None: + # Generate object by sweeping the profile along the sweeping path + if path is not None and isinstance(object, np.ndarray): + object_3d = trimesh.creation.sweep_polygon(Polygon(object), path=path, engine=ENGINE) + elif isinstance(object, trimesh.Trimesh): + object_3d = object + else: + raise RuntimeError( + "If object is a NumPy array, also a sweeping path needs to be provided. Otherwise, it is assumed" + " that object is already a mesh." + ) + + # apply object transformation + if cfg.object_transformation is not None: + T1 = cfg.object_transformation(length=object_length, width=width, params=object_values, cfg=cfg) + T2 = transformations.translation(vec=object_location) + T = trimesh.transformations.concatenate_matrices(T1, T2) + else: + T = transformations.translation(vec=object_location) + object_3d.apply_transform(T) + + # color the object + color_mesh( + cfg=cfg, + mesh=object_3d, + segment="trail_object", + ) + # Store object mesh and its x-anchor. Twist is applied after trail + # roll angles are known for the full path. + object_anchor_x = object_location[0] + 0.5 * object_length + object_meshes.append((object_3d, object_anchor_x)) + + # add local terrain underneath the object + if cfg.trail_under_object is not None: + local_path = cfg.trail_under_object(length=object_length, width=width, params=object_values, cfg=cfg) + local_path += object_location # move to current object + object_location[2] += local_path[-1, 2] - local_path[0, 2] # adjust height + elif cfg.extrude_trail_objects: + s = np.linspace(0.0, 1.0, cfg.num_segments) * object_length + local_path = np.stack([s, s * 0.0, s * 0.0], axis=1) + local_path += object_location + # add vertices at start and end of object + elif object is not None: + local_path = get_bounding_box(object_3d.vertices) + local_path[:, 1:3] = trail_path[-1, 1:3] + # no object available (flat segment) + else: + s = np.array([0.0, object_length]) + local_path = np.stack([s, s * 0.0, s * 0.0], axis=1) + local_path += object_location + local_path[:, 1:3] = trail_path[-1, 1:3] + # remember x coordinates under the object + xs = np.array([[local_path[0, 0], local_path[-1, 0]]]) + trail_x_under_object = ( + xs if trail_x_under_object is None else np.concatenate((trail_x_under_object, xs), axis=0) + ) + + # add local path only if we do not move backwards, or if the path is not located outside of the trail + for id in range(local_path.shape[0]): + if local_path[id, 0] - trail_path[-1, 0] >= EPS and local_path[id, 0] <= max_trail_size[0] - EPS: + trail_path = np.vstack([trail_path, local_path[id, :]]) + + # locate the coordinates of the proceeding object + object_location[0] += object_length + length_between_objects + object_location[1] = local_path[-1, 1] + + # add trail shape + trail_profile, border_ids, non_trail_ids = trail_profiles.trail_profile( + width=width, + thickness=cfg.thickness, + wp=cfg.wp, + ) + trail_profile[:, 0] -= 0.5 * width # center trail in the middle of the y axis + + # resample the path to enforce path resolution + trail_path = down_sample_to_enforce_min_res(trail_path=trail_path, path_res=cfg.path_res) + + # add trail roll + roll_values = interp_dict_and_sample(cp0=cfg.roll0, cp1=cfg.roll1, x=difficulty) + trail_roll = np.zeros(trail_path.shape[0]) + if cfg.roll_functions is not None: + for roll_function in cfg.roll_functions: + trail_roll += roll_function(path=trail_path, params=roll_values) + + # Apply local roll to trail objects based on interpolated trail roll. + if object_meshes: + x_path = trail_path[:, 0] + y_path = trail_path[:, 1] + z_path = trail_path[:, 2] + x_unique, unique_ids = np.unique(x_path, return_index=True) + y_unique = y_path[unique_ids] + z_unique = z_path[unique_ids] + roll_unique = trail_roll[unique_ids] + + for object_3d, object_x in object_meshes: + roll_angle = np.interp(object_x, x_unique, roll_unique) + pivot = np.array( + [ + object_x, + np.interp(object_x, x_unique, y_unique), + np.interp(object_x, x_unique, z_unique), + ] + ) + T_roll = trimesh.transformations.rotation_matrix( + angle=-roll_angle, + direction=[1.0, 0.0, 0.0], + point=pivot, + ) + object_3d.apply_transform(T_roll) + mesh += object_3d + + # create trail mesh by sweeping the trail profile along the trail path + trail_mesh = trimesh.creation.sweep_polygon( + Polygon(trail_profile), + path=trail_path, + angles=trail_roll, + engine=ENGINE, + cap=False, + ) + + # absorb objects into trail mesh + if cfg.extrude_trail_objects and not mesh.is_empty: + locations_ray, index_ray, _ = mesh.ray.intersects_location( + ray_origins=trail_mesh.vertices, + ray_directions=np.tile([0.0, 0.0, 1.0], (len(trail_mesh.vertices), 1)), + ) + vertices_z = trail_mesh.vertices[:, 2].copy() + for ray_idx, hit in zip(index_ray, locations_ray): + vertices_z[ray_idx] = max(vertices_z[ray_idx], hit[2]) + new_vertices = trail_mesh.vertices.copy() + new_vertices[:, 2] = vertices_z + trail_mesh = trimesh.Trimesh(vertices=new_vertices, faces=trail_mesh.faces) + mesh = trimesh.Trimesh() # delete objects + + # color the trail + color_mesh(cfg=cfg, mesh=trail_mesh, segment="trail") + if trail_x_under_object is not None: + for id in range(trail_x_under_object.shape[0]): + vx = trail_mesh.vertices[:, 0] + color_ids = np.where((vx > trail_x_under_object[id, 0] + EPS) & (vx < trail_x_under_object[id, 1] - EPS))[0] + color_mesh( + cfg=cfg, + mesh=trail_mesh, + segment="trail_under_object", + color_ids=color_ids, + ) + + # extend edges towards terrain border and color the floor + floor_width = cfg.floor_width if (cfg.floor_width is not None) else cfg.size[1] + mesh += process_floor( + mesh=trail_mesh, + delta_y=floor_width, + profile=trail_profile, + path=trail_path, + border_ids=border_ids, + non_trail_ids=non_trail_ids, + cfg=cfg, + ) + + # add platforms P0 and P1 + mesh, origin_P0, origin_P1 = add_platforms( + cfg=cfg, + trail_path=trail_path, + trail_roll=trail_roll, + trail_profile=trail_profile, + border_ids=border_ids, + non_trail_ids=non_trail_ids, + floor_width=floor_width, + mesh=mesh, + ) + mesh = trimesh_utils.fix_mesh(mesh) + + # adjust center (positive x is towards goal) + origin_P0[0] += cfg.distance_start_to_trail + origin_P1[0] -= cfg.distance_start_to_trail + + # add terrain outline (curvatures) + # important: we ensure that all random functions are called with the same seed + seed = random.randint(445, 1000) + closest_idx_P0 = np.argmin(np.linalg.norm(mesh.vertices - origin_P0, axis=1)) + closest_idx_P1 = np.argmin(np.linalg.norm(mesh.vertices - origin_P1, axis=1)) + if cfg.terrain_functions is not None: + for terrain_function in cfg.terrain_functions: + # check if if we should skip + if cfg.skip_terrain_functions is not None and terrain_function.func.__name__ in cfg.skip_terrain_functions: + continue + # set seed + random.seed(a=seed) + np.random.seed(seed=seed) + # compute vertex distortion + delta_mesh_vertices = terrain_function(vertices=mesh.vertices, difficulty=difficulty) + + # adjust platform origins using the closest mesh-vertex displacement + origin_P0 += delta_mesh_vertices[closest_idx_P0] + origin_P1 += delta_mesh_vertices[closest_idx_P1] + + mesh.vertices += delta_mesh_vertices + # set seed again + random.seed(a=seed) + np.random.seed(seed=seed) + # compute path distortion + delta_vertices = terrain_function(vertices=trail_path, difficulty=difficulty) + trail_path += delta_vertices + seed += random.randint(1, 100) + + # add decoration + mesh = add_decoration( + mesh=mesh, + width=width, + cfg=cfg, + trail_path=trail_path, + max_trail_size=max_trail_size, + ) + + # reverse mesh if we want to drive down + if cfg.ride_direction == "uphill": + origin = origin_P0 + elif cfg.ride_direction == "downhill": + origin = origin_P1 + + # flip the mesh + center = mesh.centroid + T_to_center = transformations.translation(vec=-center) + T_flip_x = transformations.identity() + T_flip_x[0, 0] = -1.0 + T_from_center = transformations.translation(vec=center) + T = trimesh.transformations.concatenate_matrices(T_from_center, T_flip_x, T_to_center) + mesh.apply_transform(T) + + # flip the origin + origin = trimesh.transformations.transform_points(np.array([origin]), T)[0] + + else: + raise RuntimeError("Unknown ride direction.") + + # clip the y axis of the entire trail + mesh.vertices[:, 1] = np.clip( + mesh.vertices[:, 1], + a_min=-0.5 * max_trail_size[1], + a_max=0.5 * max_trail_size[1], + ) + + # move entire trail to its starting position + trail_translation = [cfg.border_width + cfg.platform_length, 0.5 * cfg.size[1], 0.0] + mesh.apply_translation(trail_translation) + origin += trail_translation + + # clean up mesh + mesh = trimesh_utils.fix_mesh(mesh) + + # debug info + print( + "[Trail Library] type:", + cfg.__class__.__name__, + ", num vertices:", + mesh.vertices.shape[0], + ", difficulty:", + difficulty, + ) + + return mesh, origin + + +def down_sample_to_enforce_min_res(trail_path: np.ndarray, path_res: float) -> np.ndarray: + """This function adjusts the trail path s.t. + + any two adjacent points are at least path_res meters apart from each other. + Args: + trail_path: parametrization of the trail path. + path_res: resolution of the path [m]. + + Returns: + trail_path: parametrization of trail path with new resolution + """ + knot_id = 1 + while True: + p1 = trail_path[knot_id - 1] + p2 = trail_path[knot_id] + trail_heading = p2 - p1 + distance_between_knots = np.linalg.norm(trail_heading[0:2]) + if distance_between_knots > path_res: + num_additional_segments = int(np.ceil(distance_between_knots / path_res)) + 1 + xi = np.linspace(p1[0], p2[0], num_additional_segments) + yi = np.linspace(p1[1], p2[1], num_additional_segments) + zi = np.linspace(p1[2], p2[2], num_additional_segments) + si = np.stack([xi, yi, zi], axis=1) + trail_path = np.vstack([trail_path[0:knot_id], si[1:-1], trail_path[knot_id:]]) + knot_id += num_additional_segments - 2 # remove first and last point + knot_id += 1 + if knot_id >= trail_path.shape[0]: + break + return trail_path + + +def add_decoration( + mesh: trimesh.Trimesh, + width: float, + cfg: trail_cfg.TrailBaseCfg, + trail_path: np.ndarray, + max_trail_size: tuple[float, float], +) -> trimesh.Trimesh: + """This function adds decorative objects to the trail mesh. + + Warning: this function is computationally expensive. + + Args: + mesh: mesh of the trail terrain. + width: the width of the trail [m]. + cfg: The configuration of the sub-terrain. + trail_path: parametrization of the trail path. + max_trail_size: bounding box of the trail in meters. + + Returns: + mesh of the trail terrain with decorative objects added. + """ + + def get_bounding_box_floor(mesh: trimesh.Trimesh, center: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + # get x percent of lower vertices + z = mesh.vertices[:, 2] + z_min = min(z) + z_max = max(z) + percentile = 0.01 + floor_ids = np.nonzero(z <= z_min + percentile * (z_max - z_min))[0] + floor_vertices = mesh.vertices[floor_ids] + check_points = np.tile(center, (4, 1)) + check_points[0, 0] += max(floor_vertices[:, 0]) + check_points[1, 0] += min(floor_vertices[:, 0]) + check_points[2, 1] += max(floor_vertices[:, 1]) + check_points[3, 1] += min(floor_vertices[:, 1]) + return check_points, floor_ids + + decorations = trimesh.Trimesh() + trail_path_xy = trail_path[:, 0:2] + half_max_y = 0.5 * max_trail_size[1] + max_x = max_trail_size[0] + ray_directions_down = np.tile([0.0, 0.0, -1.0], (4, 1)) + + if cfg.decoration_functions is not None and cfg.rel_decorated_terrains >= np.random.rand(): + # load the list of decorative elements + decoration_path = decoration_functions.generated_path(training=cfg.training) + if not os.path.isfile(decoration_path): + decoration_functions.generate(training=cfg.training) + scene = trimesh.load(decoration_path) + list_of_objects = defaultdict(list) + # categorize objects + for object_name, object in scene.geometry.items(): + object_type = object_name.split("/")[0] + if object_type == "trees": + object_type = object_name.split("/")[1] + list_of_objects[object_type].append(object) + + # init + distance_between_decorations = sample(cfg.distance_between_decorations) + distance_between_decorations_sq = distance_between_decorations * distance_between_decorations + min_trail_to_object = cfg.wp.max_wall_width + 0.5 * width + sample(cfg.trail_border_clearance) + min_trail_to_object_eps = max(min_trail_to_object - EPS, 0.0) + min_trail_to_object_eps_sq = min_trail_to_object_eps * min_trail_to_object_eps + min_trail_to_object_sq = min_trail_to_object * min_trail_to_object + last_trail_knot = trail_path[0] + location_buffer = np.empty((0, 3)) + if cfg.decoration_function_weights is not None: + weights = np.array(cfg.decoration_function_weights) + weights = weights / weights.sum() + else: + weights = None + sampled_idx = np.random.choice(len(cfg.decoration_functions), p=weights) + decoration_function = cfg.decoration_functions[sampled_idx] + # iterate over all knots of the trail + for trail_knot in trail_path[1:]: + trail_heading = trail_knot - last_trail_knot + # check if point is sufficiently far away from previous knot. + if np.dot(trail_heading[0:2], trail_heading[0:2]) >= distance_between_decorations_sq: + # compute trail vector + trail_lateral = np.array([trail_heading[1], -trail_heading[0], 0.0]) + trail_lateral_norm = np.linalg.norm(trail_lateral) + if trail_lateral_norm <= EPS: + continue + trail_lateral /= trail_lateral_norm + for layer in range(1, cfg.num_decoration_layers + 1): + # compute location relative to the trail path + for trail_sign in [-1.0, 1.0]: + location = trail_knot + trail_sign * trail_lateral * ( + min_trail_to_object + (layer - 1.0) * distance_between_decorations + ) + + # sample decoration + decoration = decoration_function( + list_of_objects=list_of_objects, + cfg=cfg, + ) + + # conservative correction based on bounding box assumption + z_min = min(decoration.vertices[:, 2]) + check_points = decoration.vertices[ + decoration.vertices[:, 2] <= z_min + cfg.overhanging_clearance + ] + circle = minimum_bounding_circle(MultiPoint(check_points[:, 0:2]).convex_hull) + location[:2] += circle.centroid.coords[0] # center + location += trail_sign * trail_lateral * (minimum_bounding_radius(circle)) # radius + + # check if object is outside the terrain patch + if not (0.0 <= location[0] <= max_x and abs(location[1]) <= half_max_y): + continue + + # check if object is too close to other objects + if location_buffer.shape[0] > 0: + delta_locations = location_buffer[:, 0:2] - location[0:2] + d2_locations = np.einsum("ij,ij->i", delta_locations, delta_locations) + if np.any(d2_locations < distance_between_decorations_sq): + continue + + # check if object is too close to the trail + delta_to_trail = trail_path_xy - location[0:2] + d2_to_trail = np.einsum("ij,ij->i", delta_to_trail, delta_to_trail) + if np.any(d2_to_trail < min_trail_to_object_eps_sq): + continue + + # check if outline of object does not touch the trail + check_points_xy = check_points[:, 0:2] + location[0:2] + distances_to_path = ( + trail_path_xy[:, None, :] - check_points_xy[None, :, :] + ) # (N, 1, 2) - (1, M, 2) -> (N, M, 2) + d2_to_outline = np.einsum("ijk,ijk->ij", distances_to_path, distances_to_path) + if np.any(d2_to_outline < min_trail_to_object_sq): + continue + # project decoration onto ground + check_points, floor_ids = get_bounding_box_floor(mesh=decoration, center=location) + check_points[:, 2] += 100.0 + intersections, _, _ = mesh.ray.intersects_location( + ray_origins=check_points, + ray_directions=ray_directions_down, + multiple_hits=False, + ) + if intersections.size == 0: + continue + # apply transformation + location[2] = min(intersections[:, 2]) - 0.0 + decoration.vertices[floor_ids, 2] -= 0.2 # conservative correction to close any gaps + decoration.apply_translation(location) + + # add decoration object + location_buffer = np.vstack([location_buffer, location]) + decorations += decoration + last_trail_knot = trail_knot + return mesh + decorations + + +def process_floor( + mesh: trimesh.Trimesh, + delta_y: float, + profile: np.ndarray, + path: np.ndarray, + border_ids: np.ndarray, + non_trail_ids: np.ndarray, + cfg: trail_cfg.TrailBaseCfg, +) -> trimesh.Trimesh: + """This function post processes the mesh. + + It does so in two steps: 1. It extends vertices of the mesh on the very left and very right towards the outside. + 2. It colors the extended parts with the floor color plate. + + Args: + mesh: mesh to be manipulated. + delta_y: the vertices are moved by this amount along the y axis [m]. + profile: the profile used to sweep the path. + path: path used for sweeping the profile. + border_ids: indices indicating the left and right side of the trail profile. + non_trail_ids: indices indicating all vertices not belonging to the trail profile. + cfg: The configuration of the sub-terrain. + + Returns: + mesh with extended vertices on the left and right. + """ + if delta_y > EPS: + # identify edge and surface indices + num_edges = profile.shape[0] + left_border_ids = np.zeros(path.shape[0] * 2, dtype=int) + right_border_ids = np.zeros(path.shape[0] * 2, dtype=int) + + num_non_trail_points = len(non_trail_ids) + color_ids = np.zeros(path.shape[0] * num_non_trail_points, dtype=int) + for id in range(path.shape[0]): + left_border_ids[2 * id : 2 * id + 2] = border_ids[0:2] + id * num_edges + right_border_ids[2 * id : 2 * id + 2] = border_ids[2:4] + id * num_edges + color_ids[num_non_trail_points * id : num_non_trail_points * id + num_non_trail_points] = ( + non_trail_ids + id * num_edges + ) + + # move border indices to the border + mesh.vertices[left_border_ids, 1] -= delta_y + mesh.vertices[right_border_ids, 1] += delta_y + + # sample a random floor color + color_mesh( + cfg=cfg, + mesh=mesh, + segment="floor", + color_ids=color_ids, + ) + + return mesh + + +def add_platforms( + cfg: trail_cfg.TrailBaseCfg, + trail_path: np.ndarray, + trail_roll: np.ndarray, + trail_profile: np.ndarray, + border_ids: np.ndarray, + non_trail_ids: np.array, + floor_width: float, + mesh: trimesh.Trimesh, +) -> tuple[trimesh.Trimesh, np.ndarray, np.ndarray]: + """Add two platform meshes at the start and end of the trail. + + Args: + cfg: The configuration of the sub-terrain. + trail_path: parametrization of the trail path. + trail_roll: roll angles along the trail path. + trail_profile: profile of the trail. + border_ids: indices indicating the left and right side of the trail profile. + non_trail_ids: indices indicating all vertices not belonging to the trail profile. + floor_width: width of the non trail floor. + mesh: mesh of the trail terrain and the origin of the first platform. + + Returns: + A tuple of (mesh_with_platforms, origin_P0, origin_P1) where origin_P0 is the world-frame + origin of the first platform and origin_P1 is the world-frame origin of the second platform. + """ + platform_width = cfg.platform_length if (cfg.floor_width is not None) else floor_width + origin_P0 = np.array( + [ + trail_path[0, 0] - 0.5 * cfg.platform_length, + trail_path[0, 1], + trail_path[0, 2], + ] + ) + origin_P1 = np.array( + [ + trail_path[-1, 0] + 0.5 * cfg.platform_length, + trail_path[-1, 1], + trail_path[-1, 2], + ] + ) + P0 = "goal" if cfg.ride_direction == "downhill" else "start" + P1 = "start" if cfg.ride_direction == "downhill" else "goal" + platform_defs = [ + # bottom platform + ( + P0, + np.vstack( + [ + origin_P0 - np.array([0.5 * cfg.platform_length, 0, 0]), + origin_P0 + np.array([0.5 * cfg.platform_length, 0, 0]), + ] + ), + trail_roll[0], + ), + # top platform + ( + P1, + np.vstack( + [ + origin_P1 - np.array([0.5 * cfg.platform_length, 0, 0]), + origin_P1 + np.array([0.5 * cfg.platform_length, 0, 0]), + ] + ), + trail_roll[-1], + ), + ] + for segment, platform_path, platform_angle in platform_defs: + # generate platform + platform_path_downsampled = down_sample_to_enforce_min_res(trail_path=platform_path, path_res=cfg.path_res) + platform_angles = np.full(platform_path_downsampled.shape[0], platform_angle) + platform = trimesh.creation.sweep_polygon( + Polygon(trail_profile), + path=platform_path_downsampled, + angles=platform_angles, + engine=ENGINE, + cap=False, + ) + # color the trail within the platform + color_mesh(cfg=cfg, mesh=platform, segment=segment) + # process and add platform to mesh + mesh += process_floor( + mesh=platform, + delta_y=0.5 * platform_width, + profile=trail_profile, + path=platform_path_downsampled, + border_ids=border_ids, + non_trail_ids=non_trail_ids, + cfg=cfg, + ) + return (mesh, origin_P0, origin_P1) + + +def color_mesh( + cfg: trail_cfg.TrailBaseCfg, + mesh: trimesh.Trimesh, + segment: Literal["trail", "trail_object", "trail_under_object", "floor", "start", "goal"], + color_ids: np.ndarray | None = None, +): + """Color vertices of a mesh according to the configuration. + + Args: + cfg: The configuration of the sub-terrain. + mesh: mesh to be colored. + segment: the name of the segment we want to color. + color_ids: vertex indices to be colored. If None, applies to all vertices. + + Returns: + None. The function modifies `mesh` in-place. + """ + col_cfg = getattr(cfg, "col_" + segment) + if col_cfg.color_mesh: + # if no indices are provided, select all indices + if color_ids is None: + color_ids = np.arange(mesh.vertices.shape[0]) + size = (len(color_ids), 3) + color_plate = col_cfg.hsv + uniform = col_cfg.uniform + # use hsv color plate + if "h" in col_cfg.hsv: + # initialize colors (hsv) + hsv = np.ones(size) + # sample colors + for id, ic in enumerate("hsv"): + if uniform or isinstance(color_plate[ic], float): + hsv[:, id] = sample(color_plate[ic]) + else: + hsv[:, id] = np.random.uniform(*color_plate[ic], size=size[0]) + # color the vertices + mesh.visual.vertex_colors[color_ids, 0:3] = colors.hsv_to_rgb(hsv) * 255.0 + # use rgb color plate + elif "r" in col_cfg.hsv: + # initialize colors (rgb) + rgb = np.ones(size) + # sample colors + for id, ic in enumerate("rgb"): + if uniform or isinstance(color_plate[ic], float): + rgb[:, id] = sample(color_plate[ic]) * 255 + else: + rgb[:, id] = np.random.uniform(*color_plate[ic], size=size[0]) * 255 + # color the vertices + mesh.visual.vertex_colors[color_ids, 0:3] = rgb + else: + raise RuntimeError("Color plate needs to be either HSV or RGB.") diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/colors.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/colors.py new file mode 100644 index 000000000000..dd2610b38817 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/colors.py @@ -0,0 +1,177 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. +import numpy as np +import torch + +# Imported helper +from .math import in_limits + +# HSV colors +HSV_GREEN = {"h": (0.3, 0.35), "s": (0.8, 0.9), "v": (0.05, 0.15)} +HSV_BROWN = {"h": (0.05, 0.1), "s": (0.4, 0.7), "v": (0.1, 0.2)} +HSV_RED = {"h": 0.0, "s": 1.0, "v": 1.0} + +# RGB colors +RGB_RED = {"r": 1.0, "g": 0.0, "b": 0.0} +RGB_BLACK = {"r": 0.0, "g": 0.0, "b": 0.0} +RGB_RED_CHANNEL = {"r": 1.0, "g": (0.0, 1.0), "b": (0.0, 1.0)} +RGB_BLUE = {"r": 0.0, "g": 0.0, "b": 1.0} +RGB_MAGANTA = {"r": 1.0, "g": 0.0, "b": 1.0} +RGB_YELLOW = {"r": 1.0, "g": 1.0, "b": 0.0} + +# Color used for trail segmentation +TRAIL = HSV_BROWN +FLOOR = HSV_GREEN +START = HSV_BROWN +GOAL = RGB_MAGANTA + + +def rgb_to_hsv(rgb: torch.Tensor) -> torch.Tensor: + """Helper to convert an RGB tensor into an HSV tensor. + + Args: + rgb: Tensor containing normalized RGB values in [0, 1], shape (..., 3). + + Returns: + A tensor of normalized HSV values in [0, 1], shape (..., 3). + """ + # rgb: (..., 3), values in [0,1] + r, g, b = torch.unbind(rgb, dim=-1) + + maxc, _ = torch.max(rgb, dim=-1) + minc, _ = torch.min(rgb, dim=-1) + v = maxc + deltac = maxc - minc + eps = 1e-6 + + # Saturation + s = deltac / (v + eps) + + # Hue + rc = (maxc - r) / (deltac + eps) + gc = (maxc - g) / (deltac + eps) + bc = (maxc - b) / (deltac + eps) + + h = torch.zeros_like(maxc) + h[r == maxc] = bc[r == maxc] - gc[r == maxc] + h[g == maxc] = 2.0 + rc[g == maxc] - bc[g == maxc] + h[b == maxc] = 4.0 + gc[b == maxc] - rc[b == maxc] + h = (h / 6.0) % 1.0 + + return torch.stack([h, s, v], dim=-1) + + +def hsv_to_rgb(hsv: np.ndarray) -> np.ndarray: + """Helper to convert an HSV numpy array into an RGB numpy array. + + Args: + hsv: Array containing normalized HSV values in [0, 1], shape (..., 3). + + Returns: + An array of normalized RGB values in [0, 1], shape (..., 3). + """ + # hsv: (..., 3), values in [0,1] + h = hsv[..., 0] + s = hsv[..., 1] + v = hsv[..., 2] + + h6 = (h % 1.0) * 6.0 + i = np.floor(h6).astype(np.int64) + f = h6 - i.astype(h6.dtype) + + p = v * (1.0 - s) + q = v * (1.0 - s * f) + t = v * (1.0 - s * (1.0 - f)) + + i_mod = i % 6 + r = np.empty_like(v) + g = np.empty_like(v) + b = np.empty_like(v) + + mask = i_mod == 0 + r[mask], g[mask], b[mask] = v[mask], t[mask], p[mask] + + mask = i_mod == 1 + r[mask], g[mask], b[mask] = q[mask], v[mask], p[mask] + + mask = i_mod == 2 + r[mask], g[mask], b[mask] = p[mask], v[mask], t[mask] + + mask = i_mod == 3 + r[mask], g[mask], b[mask] = p[mask], q[mask], v[mask] + + mask = i_mod == 4 + r[mask], g[mask], b[mask] = t[mask], p[mask], v[mask] + + mask = i_mod == 5 + r[mask], g[mask], b[mask] = v[mask], p[mask], q[mask] + + return np.stack([r, g, b], axis=-1) + + +def is_color(rgb: torch.tensor, color_plate: dict[str, float | tuple[float, float]]) -> torch.tensor: + """Check whether RGB colors belong to a pre-specified color plate. + + Args: + rgb: Tensor containing unnormalized RGB values in [0, 1], shape (..., 3). + color_plate: The color plate to test; can be an RGB or HSV specification. + + Returns: + A boolean tensor indicating membership in the color plate. ``True`` + means the pixel/entry belongs to the color plate. + + Raises: + RuntimeError: If the color plate does not contain RGB or HSV keys. + """ + if "r" in color_plate: + return ( + in_limits(rgb[..., 0], color_plate["r"]) + & in_limits(rgb[..., 1], color_plate["g"]) + & in_limits(rgb[..., 2], color_plate["b"]) + ) + elif "h" in color_plate: + hsv = rgb_to_hsv(rgb) + h, s, v = hsv[..., 0], hsv[..., 1], hsv[..., 2] + return ( + in_limits(h, color_plate["h"], rel_tol=0.1) + & in_limits(s, color_plate["s"], rel_tol=0.1) + & in_limits(v, color_plate["v"], rel_tol=0.1) + ) + else: + raise RuntimeError("Color plate must be either RGB or HSV.") + + +def check_color_plate(color_plate: dict[str, float | tuple[float, float]]) -> bool: + """Return True if `color_plate` is a valid RGB or HSV color specification. + + Valid examples: + + RGB: {"r": 1.0, "g": 0.0, "b": 0.0} + HSV: {"h": 0.0, "s": 1.0, "v": 1.0} + """ + return all(k in color_plate for k in ("r", "g", "b")) or all(k in color_plate for k in ("h", "s", "v")) + + +def rgb_to_numpy(color_plate: dict[str, float | tuple[float, float]]) -> np.ndarray: + """Convert a scaled RGB color plate into a NumPy uint8 RGB array. + + Args: + color_plate: RGB color specification with components in [0, 1]. + + Returns: + NumPy array of unscaled RGB values (dtype uint8), e.g. [R, G, B]. + + Raises: + RuntimeError: If `color_plate` is not an RGB specification. + """ + if "r" in color_plate: + r = color_plate["r"] * 255 + g = color_plate["g"] * 255 + b = color_plate["b"] * 255 + return np.array([r, g, b], dtype=np.uint8) + else: + raise RuntimeError("Color plate must be RGB.") diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/math.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/math.py new file mode 100644 index 000000000000..f2bfc5e3696f --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/math.py @@ -0,0 +1,155 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +import random + + +def interp( + param0: float | tuple[float, float], param1: float | tuple[float, float], x: float +) -> float | tuple[float, float]: + """Linearly interpolate between ``param0`` and ``param1``. + + Each parameter may be either a float or a tuple of two floats. When a + tuple is provided the result is a tuple obtained by interpolating each + element independently (useful for representing ranges). The interpolation + is performed as ``param0 + (param1 - param0) * x`` on scalar values. + + Args: + param0: value or (min, max) for which ``x`` is zero. + param1: value or (min, max) for which ``x`` is one. + x: fraction between 0 and 1 indicating interpolation position. + + Returns: + Interpolated value; same type as the inputs (float or tuple). + + Raises: + TypeError: if ``param0`` or ``param1`` are not either float or a + 2-tuple of floats. + ValueError: if the tuple arguments are not length two. + """ + + # check validity of params + def validate(p, name: str): + if isinstance(p, float): + return + if isinstance(p, tuple): + if len(p) != 2: + raise ValueError(f"{name} tuple must have length 2, got {len(p)}") + if not all(isinstance(v, float) for v in p): + raise TypeError(f"{name} tuple elements must be floats") + return + raise TypeError(f"{name} must be float or tuple[float,float], got {type(p)}") + + validate(param0, "param0") + validate(param1, "param1") + + # check validity of x + if x < 0.0 or x > 1.0: + raise ValueError(f"x must be between 0 and 1, got {x}") + + # interpolation + if isinstance(param0, float) and isinstance(param1, float): + return param0 + (param1 - param0) * x + elif isinstance(param0, float): + return (interp(param0, param1[0], x), interp(param0, param1[1], x)) + elif isinstance(param1, float): + return (interp(param0[0], param1, x), interp(param0[1], param1, x)) + else: + return (interp(param0[0], param1[0], x), interp(param0[1], param1[1], x)) + + +def interp_dict_and_sample(cp0: object, cp1: object, x: float) -> dict[str, float]: + """Linearly interpolate dictionary of parameters. + + Args: + cp0: object containing params (explanation in notes) for which x is zero. + cp1: object containing params (explanation in notes) for which x is one. + x: value between zero and one. + + Both c0 and cp1 contain a member called "params", which are dictionaries. + The key is the corresponding name and the value is of type float or tuple[float,float]: + * float: a single value + * tuple[float, float]: the limits of a uniform distribution + + Returns: + a dictionary of interpolated and sampled values. + + Raises: + RuntimeError: If the arguments are not of the correct type. + """ + # Ensure that arguments contain a `params` attribute + if not hasattr(cp0, "params") or not hasattr(cp1, "params"): + raise RuntimeError("cp0 or cp1 do not contain parameters that can be interpolated.") + # Ensure that `params` is a dictionary + if not isinstance(cp0.params, dict) or not isinstance(cp1.params, dict): + raise RuntimeError("params must be a dict.") + # Interpolate each parameter and sample if necessary + values: dict[str, float] = {} + for name in cp0.params.keys(): + values[name] = sample(interp(param0=cp0.params[name], param1=cp1.params[name], x=x)) + return values + + +def sample(limits: float | int | tuple[float | int, float | int]) -> float | int: + """If ``limits`` is a tuple, sample uniformly from U(min, max). + + If ``limits`` is a float or int, return it unchanged. + + Note: booleans are treated as integers. + + Args: + limits: Either a scalar value or a (min, max) pair specifying the + uniform sampling range. + + Returns: + A single sampled or forwarded value (int or float). + """ + # return single value + if not isinstance(limits, tuple): + return limits + # sample random int or float + elif isinstance(limits[0], int): + return int(round(random.uniform(*limits))) + # sample random float + return random.uniform(*limits) + + +def sample_sign() -> float: + """Sample +1 or -1 with equal probability.""" + return 1.0 if random.uniform(0.0, 1.0) > 0.5 else -1.0 + + +def in_limits( + value: object, + limits: float | int | tuple[float | int, float | int], + rel_tol: float = 0.0, +) -> object: + """Test if value is within specified limits. + + If ``limits`` is a tuple, check whether ``value`` is within + [limits[0], limits[1]] (inclusive). If ``limits`` is a scalar, check + whether ``value`` equals ``limits`` within the specified relative + tolerance. + + Note: This function assumes that the type of ``value`` implements the + ``>=`` and ``<=`` operators against float or int (or is a NumPy / torch + array supporting elementwise comparisons). + + Args: + value: The value to check. + limits: Either a scalar or a (min, max) pair containing lower and + upper bounds. + rel_tol: Relative tolerance. Default is 0.0. + + Returns: + A boolean or boolean-like object indicating whether ``value`` is + within the specified limits. + """ + if isinstance(limits, tuple): + return (value >= limits[0] * (1.0 - rel_tol)) & (value <= limits[1] * (1.0 + rel_tol)) + else: + return (value >= limits * (1.0 - rel_tol)) & (value <= limits * (1.0 + rel_tol)) diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/numpy_arrays.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/numpy_arrays.py new file mode 100644 index 000000000000..af2f372f3faa --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/numpy_arrays.py @@ -0,0 +1,108 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +"""NumPy helper functions for trail generation. + +These helpers operate on NumPy arrays describing 2D/N-D geometric profiles used by the trail generator. +""" + +import numpy as np + + +def mirror_and_join(object: np.ndarray, dim: int = 0, dim_flip: int = -1, offset: float = 0.0) -> np.ndarray: + """Mirror an array along one axis and join it with an offset. + + Args: + object: Array of shape (N, m) to be mirrored and extended. + dim: Dimension index to be extended (default 0). Use -1 for the last + dimension. + dim_flip: Dimension index to flip/mirror (default -1 for last dim). + offset: Offset to add between the original array and its mirrored copy. + + Note: + It is possible to set ``dim == dim_flip`` in principle, but this is + not the intended use case. Only a single axis is mirrored, and the + flip is performed on the component specified by ``dim_flip``. + + Returns: + A new NumPy array containing the original array stacked with its + mirrored counterpart. + + Raises: + RuntimeError: if ``object`` does not have shape (N, m) or if ``dim`` is + not a valid column index for the array. + """ + if len(object.shape) != 2 or dim >= object.shape[1]: + raise RuntimeError("Array has the wrong shape. Expect an array of shape (N, m) with dim < m.") + mirror = object.copy() + mirror[:, dim_flip] = np.flip(mirror[:, dim_flip]) + mirror[:, dim] += abs(object[0, dim] - object[-1, dim]) + offset + return np.vstack([object, mirror]) + + +def get_bounding_box(object: np.ndarray) -> np.ndarray: + """Compute an axis-aligned bounding box for an array of points. + + Args: + object: Array of shape (N, m) representing N points in m dimensions. + + Returns: + Array of shape (2, m) where the first row contains minima and the + second row contains maxima for each column. + + Raises: + RuntimeError: if ``object`` is not a 2D array of shape (N, m). + """ + if len(object.shape) != 2: + raise RuntimeError("Array has the wrong shape. Expect an array of shape (N, m).") + object_linearized = np.zeros((2, object.shape[1])) + object_linearized[0, :] = np.min(object, axis=0) + object_linearized[1, :] = np.max(object, axis=0) + return object_linearized + + +def decay_at_boundaries(object: np.ndarray, vec: np.ndarray | None, dim: int = 0, threshold: float = 1.0): + """Decay values toward zero at the boundaries along a specified dimension. + + The function scales the values in ``object[:, dim]`` near the lower and + upper boundaries (as defined by ``threshold`` on ``vec``) so they smoothly + approach zero. The operation is performed in-place on ``object``. + + Args: + object: 1D array of length N or 2D array with shape (N, m). If a 1D + array is provided it will be treated as shape (N, 1). + vec: Reference vector used to determine boundary regions. If ``None``, + ``object[:, 0]`` is used. + dim: Column index to decay (default 0). + threshold: Boundary width in the same units as ``vec``. Values with + ``vec < threshold`` or ``vec > max(vec) - threshold`` will be + decayed. Default is 1.0. + + Returns: + The modified ``object`` (same array, modified in-place). + + Raises: + RuntimeError: if ``object`` is neither 1D nor 2D. + """ + if len(object.shape) == 1: + object = np.expand_dims(object, axis=1) + dim = 0 + if len(object.shape) != 2: + raise RuntimeError("Array has the wrong shape. Expect an array of shape (N, m).") + + if vec is None: + vec = object[:, 0] + + # Lower boundary: scale values where vec < threshold (range [0 .. threshold]) + ids = np.nonzero(vec < threshold) + object[ids, dim] *= vec[ids] / threshold + + # Upper boundary: scale values where vec > max(vec) - threshold + max_vec = np.max(vec) + ids = np.nonzero(vec > (max_vec - threshold)) + decay = (vec[ids] - (max_vec - threshold)) / threshold + object[ids, dim] *= 1.0 - decay diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/transformations.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/transformations.py new file mode 100644 index 000000000000..6d1b0ebe84b5 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/transformations.py @@ -0,0 +1,76 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +"""Utilities to build 4x4 homogeneous transforms using the trimesh helpers. + +For more information see: + https://trimesh.org/trimesh.transformations.html +""" + +import numpy as np +import trimesh + + +def identity() -> np.ndarray: + """Returns a transform that maps an object into itself.""" + return trimesh.transformations.identity_matrix() + + +def translation(vec: tuple[float, float, float]) -> np.ndarray: + """Return a transform that translates an object. + + Args: + vec: Translation vector (x, y, z). + """ + return trimesh.transformations.translation_matrix(vec) + + +def roll(angle: float) -> np.ndarray: + """Return a transform that rotates (rolls) the object about the X axis. + + Args: + angle: Roll angle in radians. + """ + return trimesh.transformations.rotation_matrix(angle, [1.0, 0.0, 0.0]) + + +def yaw(angle: float) -> np.ndarray: + """Return a transform that rotates (yaws) the object about the Z axis. + + Args: + angle: Yaw angle in radians. + """ + return trimesh.transformations.rotation_matrix(angle, [0.0, 0.0, 1.0]) + + +def translate_and_roll(vec: tuple[float, float, float], angle: float) -> np.ndarray: + """Return a transform that translates then rolls the object. + + Args: + vec: Translation vector (x, y, z). + angle: Roll angle in radians. + """ + T1 = translation(vec) + T2 = roll(angle=angle) + return trimesh.transformations.concatenate_matrices(T1, T2) + + +def scale(vec: tuple[float, float, float]) -> np.ndarray: + """Return a non-uniform scaling transform. + + Args: + vec: Scale factors along (x, y, z) axes. + + Returns: + A 4x4 homogeneous transform that scales coordinates by the given + factors. + """ + T = identity() + T[0, 0] = vec[0] + T[1, 1] = vec[1] + T[2, 2] = vec[2] + return T diff --git a/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/trimesh_utils.py b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/trimesh_utils.py new file mode 100644 index 000000000000..28a1664a32e9 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/terrains/trail/utils/trimesh_utils.py @@ -0,0 +1,116 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2025-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +from typing import Literal + +import numpy as np +import open3d as o3d +import trimesh +from scipy.spatial import cKDTree + + +def fix_mesh(mesh: trimesh.Trimesh) -> trimesh.Trimesh: + """Repair a mesh by addressing common issues (holes, duplicated vertices). + + Args: + mesh: The mesh to repair. + + Returns: + The repaired mesh. + """ + mesh.merge_vertices() + trimesh.repair.fill_holes(mesh) + mesh.merge_vertices(merge_tex=True, merge_norm=True) + mesh.remove_unreferenced_vertices() + mesh.update_faces(mesh.unique_faces()) + return mesh + + +def simplify_mesh( + mesh: trimesh.Trimesh, + method: Literal[ + "vertex_clustering", + "relative_quadric_decimation", + "absolute_quadric_decimation", + ], + parameter: float | int, +) -> trimesh.Trimesh: + """Simplify a mesh by reducing vertices while preserving shape and vertex colors where possible. + + Args: + mesh: The input mesh to simplify. + method: Simplification method to use. Supported values: + - ``vertex_clustering``: uses a voxel grid; parameter is ``voxel_size``. + - ``relative_quadric_decimation``: parameter is relative target + fraction of triangles (0..1). + - ``absolute_quadric_decimation``: parameter is target number of + triangles. + parameter: Method-specific parameter (see description above). + + Returns: + The simplified mesh as a :class:`trimesh.Trimesh`. + """ + # Convert to Open3D mesh + mesh_o3d = o3d.geometry.TriangleMesh( + o3d.utility.Vector3dVector(mesh.vertices), + o3d.utility.Vector3iVector(mesh.faces), + ) + + # Simplify + if method == "vertex_clustering": + mesh_simplified = mesh_o3d.simplify_vertex_clustering( + voxel_size=parameter, + contraction=o3d.geometry.SimplificationContraction.Average, + ) + elif method == "relative_quadric_decimation": + mesh_simplified = mesh_o3d.simplify_quadric_decimation(int(len(mesh.faces) * parameter)) + elif method == "absolute_quadric_decimation": + mesh_simplified = mesh_o3d.simplify_quadric_decimation(parameter) + else: + raise RuntimeError("Unknown simplification method.") + + # Interpolate colors from the original mesh to the simplified vertices + # using nearest-neighbor in vertex space. + vertices_simplified = np.asarray(mesh_simplified.vertices) + tree = cKDTree(np.asarray(mesh.vertices)) + _, indices = tree.query(vertices_simplified) + colors_simplified = mesh.visual.vertex_colors[indices, :3] + + # Convert back to Trimesh + return trimesh.Trimesh( + vertices=vertices_simplified, + faces=np.asarray(mesh_simplified.triangles), + vertex_colors=colors_simplified, + process=True, + ) + + +def cut_above(mesh: trimesh.Trimesh, height: float) -> trimesh.Trimesh: + """Remove vertices above a specified height and cap the mesh, preserving colors. + + Args: + mesh: Input mesh to slice. + height: Cut plane height (units match mesh coordinates). Vertices with + z > height are removed/capped. + + Returns: + The sliced mesh with vertex colors recovered from the original mesh. + """ + # cut + mesh_sliced = mesh.slice_plane( + plane_origin=np.array([0.0, 0.0, height]), + plane_normal=np.array([0.0, 0.0, -1.0]), + cap=True, + ) + + # recover colors + vertices_sliced = np.asarray(mesh_sliced.vertices) + tree = cKDTree(np.asarray(mesh.vertices)) + _, indices = tree.query(vertices_sliced) + mesh_sliced.visual.vertex_colors = mesh.visual.vertex_colors[indices] + + return mesh_sliced diff --git a/source/isaaclab_contrib/test/terrains/test_trail.py b/source/isaaclab_contrib/test/terrains/test_trail.py new file mode 100644 index 000000000000..52dc4f163193 --- /dev/null +++ b/source/isaaclab_contrib/test/terrains/test_trail.py @@ -0,0 +1,200 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2024-2026 Robotics and AI Institute LLC dba RAI Institute. All rights reserved. + +"""Unit test for testing utility functions of the trail library. + +.. code-block:: bash + # Usage + ./isaaclab.sh -p -m pytest source/isaaclab_contrib/test/terrains/test_trail.py +""" + +import numpy as np + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import unittest + +import torch +from isaaclab_contrib.terrains.trail.utils import colors, math, numpy_arrays, transformations + + +class TestTrailUtilities(unittest.TestCase): + """Test fixture for checking trail utilities.""" + + def test_mirror_and_join(self): + """Test mirror_and_join method.""" + xi = [0.0, 0.1, 0.2, 0.3] + yi = [-0.5, -0.2, 1.2, 1.0] + zi = [1.0, 2.0, 3.0, 4.0] + object = np.stack([xi, yi, zi], axis=1) + offset = 1.0 + mirrored_object = numpy_arrays.mirror_and_join(object=object, dim=0, dim_flip=-1, offset=offset) + # check x (mirrored axis) + for dim_x in range(len(xi)): + self.assertEqual(mirrored_object[4 + dim_x, 0], xi[3] + offset + dim_x * 0.1) + # check y (should remain constant) + for dim_y in range(len(yi)): + self.assertEqual(mirrored_object[4 + dim_y, 1], yi[dim_y]) + # check z (should be flipped) + for dim_z in range(len(zi)): + self.assertEqual(mirrored_object[4 + dim_z, 2], zi[3 - dim_z]) + + def test_get_bounding_box(self): + """Test get_bounding_box method.""" + xi = [0.1, 0.5, 0.8, 10.0] + yi = [100.0, -99.0, -5.7, 170.0] + zi = [-0.2, 1.0, 1.0, 0.0] + object = np.stack([xi, yi, zi], axis=1) + bounding_box = numpy_arrays.get_bounding_box(object=object) + # check x + self.assertEqual(bounding_box[0, 0], 0.1) + self.assertEqual(bounding_box[1, 0], 10.0) + # check y + self.assertEqual(bounding_box[0, 1], -99.0) + self.assertEqual(bounding_box[1, 1], 170.0) + # check z + self.assertEqual(bounding_box[0, 2], -0.2) + self.assertEqual(bounding_box[1, 2], 1.0) + + def test_decay_at_boundaries(self): + """Test decay_at_boundaries method.""" + xi = np.linspace(0.0, 10.0, 11) + yi = xi * 0.0 + 1.0 + zi = xi * 0.0 + 1.0 + object = np.stack([xi, yi, zi], axis=1) + numpy_arrays.decay_at_boundaries(object=object, vec=None, dim=1, threshold=2.0) + numpy_arrays.decay_at_boundaries(object=object, vec=None, dim=2, threshold=3.0) + + # check y + self.assertAlmostEqual(object[0, 1], 0.0, places=5) + self.assertAlmostEqual(object[1, 1], 0.5, places=5) + for n in range(7): + self.assertAlmostEqual(object[2 + n, 1], 1.0, places=5) + self.assertAlmostEqual(object[9, 1], 0.5, places=5) + self.assertAlmostEqual(object[10, 1], 0.0, places=5) + + # check z + self.assertAlmostEqual(object[0, 2], 0.0, places=5) + self.assertAlmostEqual(object[1, 2], 1.0 / 3.0, places=5) + self.assertAlmostEqual(object[2, 2], 2.0 / 3.0, places=5) + for n in range(5): + self.assertAlmostEqual(object[3 + n, 1], 1.0, places=5) + self.assertAlmostEqual(object[8, 2], 2.0 / 3.0, places=5) + self.assertAlmostEqual(object[9, 2], 1.0 / 3.0, places=5) + self.assertAlmostEqual(object[10, 2], 0.0, places=5) + + def test_transformation(self): + """Test transformation methods.""" + vec = [0.1, 0.3, -0.8, 1.0] + + # check translation + T = transformations.translation(vec=[0.5, 0.4, 0.3]) + vecT = T.dot(vec) + self.assertAlmostEqual(vecT[0], 0.6, places=5) + self.assertAlmostEqual(vecT[1], 0.7, places=5) + self.assertAlmostEqual(vecT[2], -0.5, places=5) + + # check roll + T = transformations.roll(angle=np.pi) + vecT = T.dot(vec) + self.assertAlmostEqual(vecT[0], vec[0], places=5) + self.assertAlmostEqual(vecT[1], -vec[1], places=5) + self.assertAlmostEqual(vecT[2], -vec[2], places=5) + + # check translate and roll + T = transformations.translate_and_roll(vec=[0.5, 0.4, 0.3], angle=np.pi) + vecT = T.dot(vec) + self.assertAlmostEqual(vecT[0], 0.6, places=5) + self.assertAlmostEqual(vecT[1], -vec[1] + 0.4, places=5) + self.assertAlmostEqual(vecT[2], -vec[2] + 0.3, places=5) + + def test_interp(self): + """Test interp method.""" + # check two floats + self.assertAlmostEqual(math.interp(param0=1.2, param1=1.4, x=0.0), 1.2, places=5) + self.assertAlmostEqual(math.interp(param0=1.2, param1=1.4, x=0.5), 1.3, places=5) + self.assertAlmostEqual(math.interp(param0=1.2, param1=1.4, x=1.0), 1.4, places=5) + + # check tuple[float] and float + value = math.interp(param0=(1.0, 2.0), param1=3.0, x=0.5) + self.assertAlmostEqual(value[0], 2.0, places=5) + self.assertAlmostEqual(value[1], 2.5, places=5) + + # check two float and tuples + value = math.interp(param0=1.0, param1=(2.0, 3.0), x=0.5) + self.assertAlmostEqual(value[0], 1.5, places=5) + self.assertAlmostEqual(value[1], 2.0, places=5) + + # check two tuples + value = math.interp(param0=(1.0, 2.0), param1=(2.0, 3.0), x=0.5) + self.assertAlmostEqual(value[0], 1.5, places=5) + self.assertAlmostEqual(value[1], 2.5, places=5) + + def test_sample(self): + """Test sample method.""" + # check float + self.assertEqual(math.sample(1.2), 1.2) + # check tuple[float] + self.assertEqual(math.sample((1.2, 1.2)), 1.2) + # check int + self.assertEqual(math.sample(1), 1) + # check tuple[int] + self.assertEqual(math.sample((1, 1)), 1) + + def test_in_limits(self): + """Test in_limits method.""" + # check float + self.assertTrue(math.in_limits(value=1.5, limits=(1.0, 2.0))) + self.assertFalse(math.in_limits(value=1.5, limits=(1.0, 1.4))) + self.assertTrue(math.in_limits(value=1.5, limits=1.5)) + self.assertFalse(math.in_limits(value=1.5, limits=1.4)) + # check int + self.assertTrue(math.in_limits(value=7, limits=(-5, 8))) + self.assertFalse(math.in_limits(value=7, limits=(9, 10))) + self.assertTrue(math.in_limits(value=7, limits=7)) + self.assertFalse(math.in_limits(value=7, limits=-7)) + + def test_sample_sign(self): + """Test sample_sign method.""" + self.assertEqual(abs(math.sample_sign()), 1.0) + + def test_rgb_hsv_conversion(self): + """Test rgb_to_hsv and hsv_to_rgb with known values and round-trip checks.""" + rgb = torch.tensor( + [ + [1.0, 0.0, 0.0], # red + [0.0, 1.0, 0.0], # green + [0.0, 0.0, 1.0], # blue + [1.0, 1.0, 0.0], # yellow + [1.0, 0.0, 1.0], # magenta + ], + dtype=torch.float32, + ) + + expected_hsv = np.array( + [ + [0.0, 1.0, 1.0], + [1.0 / 3.0, 1.0, 1.0], + [2.0 / 3.0, 1.0, 1.0], + [1.0 / 6.0, 1.0, 1.0], + [5.0 / 6.0, 1.0, 1.0], + ], + dtype=np.float32, + ) + + hsv = colors.rgb_to_hsv(rgb).cpu().numpy() + np.testing.assert_allclose(hsv, expected_hsv, atol=1e-5, rtol=1e-5) + + rgb_roundtrip = colors.hsv_to_rgb(hsv) + np.testing.assert_allclose(rgb_roundtrip, rgb.cpu().numpy(), atol=1e-5, rtol=1e-5) + + +if __name__ == "__main__": + unittest.main() diff --git a/source/isaaclab_tasks/changelog.d/fjenelten-feature-trail_library_migration.rst b/source/isaaclab_tasks/changelog.d/fjenelten-feature-trail_library_migration.rst new file mode 100644 index 000000000000..617eff4ea59a --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/fjenelten-feature-trail_library_migration.rst @@ -0,0 +1,4 @@ +Added +^^^^^ + +* Added manager-based ANYmal-C trail environment as demonstration for the trail terrains, introducing :class:`~isaaclab_tasks.contrib.trail.anymal_trail_env_cfg.AnymalCTrailEnvCfg` and wiring the scene terrain to use ``TRAIL_CFG``. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/trail/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/trail/__init__.py new file mode 100644 index 000000000000..2d0e41f437da --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/trail/__init__.py @@ -0,0 +1,18 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Manager-based ANYmal-C task using the contributed trail terrains.""" + +import gymnasium as gym + +# Register the environment so it is discovered with the other Isaac Lab tasks. +gym.register( + id="IsaacContrib-Velocity-Trail-AnymalC", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.anymal_trail_env_cfg:AnymalCTrailEnvCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/trail/anymal_trail_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/trail/anymal_trail_env_cfg.py new file mode 100644 index 000000000000..82a537c83def --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/trail/anymal_trail_env_cfg.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Manager-based configuration for ANYmal-C on the contributed trail terrains. +Note that the training environment is only an example of how to integrate the trail terrains. +The MDP formulation in the current tasks may not solve any particular problem statement, but rather serves as an example for the terrain integration. +.. code-block:: bash + # Example Usage + /isaaclab.sh -p scripts/environments/zero_agent.py --task IsaacContrib-Velocity-Trail-AnymalC --num_envs 4 +""" + +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils.configclass import configclass +from isaaclab_tasks.contrib.velocity.config.anymal_c.rough_env_cfg import AnymalCRoughEnvCfg + +from isaaclab_contrib.terrains.trail.examples.trails import TRAIL_CFG + + +@configclass +class AnymalCTrailEnvCfg(AnymalCRoughEnvCfg): + """ManagerBasedRLEnvCfg for ANYmal-C walking over trail terrains.""" + + def __post_init__(self) -> None: + """Configure ANYmal-C and replace the rough terrain with trail terrain.""" + super().__post_init__() + for trail_cfg in TRAIL_CFG.sub_terrains.values(): + trail_cfg.training = False + trail_cfg.cut_objects_above = None + self.scene.terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="generator", + terrain_generator=TRAIL_CFG, + collision_group=-1, + visual_material=None, + debug_vis=False, + )