Creating Visualization Markers

Creating Visualization Markers#

Visualization markers are useful to debug the state of the environment. They can be used to visualize the frames, commands, and other information in the simulation.

While Isaac Sim provides its own omni.isaac.debug_draw extension, it is limited to rendering only points, lines and splines. For cases, where you need to render more complex shapes, you can use the markers.VisualizationMarkers class.

This guide is accompanied by a sample script markers.py in the IsaacLab/source/standalone/demos directory.

Code for markers.py
  1# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
  2# All rights reserved.
  3#
  4# SPDX-License-Identifier: BSD-3-Clause
  5
  6"""This script demonstrates different types of markers.
  7
  8.. code-block:: bash
  9
 10    # Usage
 11    ./isaaclab.sh -p source/standalone/demos/markers.py
 12
 13"""
 14
 15"""Launch Isaac Sim Simulator first."""
 16
 17import argparse
 18
 19from omni.isaac.lab.app import AppLauncher
 20
 21# add argparse arguments
 22parser = argparse.ArgumentParser(description="This script demonstrates different types of markers.")
 23# append AppLauncher cli args
 24AppLauncher.add_app_launcher_args(parser)
 25# parse the arguments
 26args_cli = parser.parse_args()
 27
 28# launch omniverse app
 29app_launcher = AppLauncher(args_cli)
 30simulation_app = app_launcher.app
 31
 32"""Rest everything follows."""
 33
 34import torch
 35
 36import omni.isaac.lab.sim as sim_utils
 37from omni.isaac.lab.markers import VisualizationMarkers, VisualizationMarkersCfg
 38from omni.isaac.lab.sim import SimulationContext
 39from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR
 40from omni.isaac.lab.utils.math import quat_from_angle_axis
 41
 42
 43def define_markers() -> VisualizationMarkers:
 44    """Define markers with various different shapes."""
 45    marker_cfg = VisualizationMarkersCfg(
 46        prim_path="/Visuals/myMarkers",
 47        markers={
 48            "frame": sim_utils.UsdFileCfg(
 49                usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd",
 50                scale=(0.5, 0.5, 0.5),
 51            ),
 52            "arrow_x": sim_utils.UsdFileCfg(
 53                usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/arrow_x.usd",
 54                scale=(1.0, 0.5, 0.5),
 55                visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 1.0)),
 56            ),
 57            "cube": sim_utils.CuboidCfg(
 58                size=(1.0, 1.0, 1.0),
 59                visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)),
 60            ),
 61            "sphere": sim_utils.SphereCfg(
 62                radius=0.5,
 63                visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)),
 64            ),
 65            "cylinder": sim_utils.CylinderCfg(
 66                radius=0.5,
 67                height=1.0,
 68                visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)),
 69            ),
 70            "cone": sim_utils.ConeCfg(
 71                radius=0.5,
 72                height=1.0,
 73                visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 1.0, 0.0)),
 74            ),
 75            "mesh": sim_utils.UsdFileCfg(
 76                usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd",
 77                scale=(10.0, 10.0, 10.0),
 78            ),
 79            "mesh_recolored": sim_utils.UsdFileCfg(
 80                usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd",
 81                scale=(10.0, 10.0, 10.0),
 82                visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.25, 0.0)),
 83            ),
 84            "robot_mesh": sim_utils.UsdFileCfg(
 85                usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd",
 86                scale=(2.0, 2.0, 2.0),
 87                visual_material=sim_utils.GlassMdlCfg(glass_color=(0.0, 0.1, 0.0)),
 88            ),
 89        },
 90    )
 91    return VisualizationMarkers(marker_cfg)
 92
 93
 94def main():
 95    """Main function."""
 96    # Load kit helper
 97    sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)
 98    sim = SimulationContext(sim_cfg)
 99    # Set main camera
100    sim.set_camera_view([0.0, 18.0, 12.0], [0.0, 3.0, 0.0])
101
102    # Spawn things into stage
103    # Lights
104    cfg = sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
105    cfg.func("/World/Light", cfg)
106
107    # create markers
108    my_visualizer = define_markers()
109
110    # define a grid of positions where the markers should be placed
111    num_markers_per_type = 5
112    grid_spacing = 2.0
113    # Calculate the half-width and half-height
114    half_width = (num_markers_per_type - 1) / 2.0
115    half_height = (my_visualizer.num_prototypes - 1) / 2.0
116    # Create the x and y ranges centered around the origin
117    x_range = torch.arange(-half_width * grid_spacing, (half_width + 1) * grid_spacing, grid_spacing)
118    y_range = torch.arange(-half_height * grid_spacing, (half_height + 1) * grid_spacing, grid_spacing)
119    # Create the grid
120    x_grid, y_grid = torch.meshgrid(x_range, y_range, indexing="ij")
121    x_grid = x_grid.reshape(-1)
122    y_grid = y_grid.reshape(-1)
123    z_grid = torch.zeros_like(x_grid)
124    # marker locations
125    marker_locations = torch.stack([x_grid, y_grid, z_grid], dim=1)
126    marker_indices = torch.arange(my_visualizer.num_prototypes).repeat(num_markers_per_type)
127
128    # Play the simulator
129    sim.reset()
130    # Now we are ready!
131    print("[INFO]: Setup complete...")
132
133    # Yaw angle
134    yaw = torch.zeros_like(marker_locations[:, 0])
135    # Simulate physics
136    while simulation_app.is_running():
137        # rotate the markers around the z-axis for visualization
138        marker_orientations = quat_from_angle_axis(yaw, torch.tensor([0.0, 0.0, 1.0]))
139        # visualize
140        my_visualizer.visualize(marker_locations, marker_orientations, marker_indices=marker_indices)
141        # roll corresponding indices to show how marker prototype can be changed
142        if yaw[0].item() % (0.5 * torch.pi) < 0.01:
143            marker_indices = torch.roll(marker_indices, 1)
144        # perform step
145        sim.step()
146        # increment yaw
147        yaw += 0.01
148
149
150if __name__ == "__main__":
151    # run the main function
152    main()
153    # close sim app
154    simulation_app.close()

Configuring the markers#

The VisualizationMarkersCfg class provides a simple interface to configure different types of markers. It takes in the following parameters:

  • prim_path: The corresponding prim path for the marker class.

  • markers: A dictionary specifying the different marker prototypes handled by the class. The key is the name of the marker prototype and the value is its spawn configuration.

Note

In case the marker prototype specifies a configuration with physics properties, these are removed. This is because the markers are not meant to be simulated.

Here we show all the different types of markers that can be configured. These range from simple shapes like cones and spheres to more complex geometries like a frame or arrows. The marker prototypes can also be configured from USD files.

marker_cfg = VisualizationMarkersCfg(
    prim_path="/Visuals/myMarkers",
    markers={
        "frame": sim_utils.UsdFileCfg(
            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd",
            scale=(0.5, 0.5, 0.5),
        ),
        "arrow_x": sim_utils.UsdFileCfg(
            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/arrow_x.usd",
            scale=(1.0, 0.5, 0.5),
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 1.0)),
        ),
        "cube": sim_utils.CuboidCfg(
            size=(1.0, 1.0, 1.0),
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)),
        ),
        "sphere": sim_utils.SphereCfg(
            radius=0.5,
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)),
        ),
        "cylinder": sim_utils.CylinderCfg(
            radius=0.5,
            height=1.0,
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)),
        ),
        "cone": sim_utils.ConeCfg(
            radius=0.5,
            height=1.0,
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 1.0, 0.0)),
        ),
        "mesh": sim_utils.UsdFileCfg(
            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd",
            scale=(10.0, 10.0, 10.0),
        ),
        "mesh_recolored": sim_utils.UsdFileCfg(
            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd",
            scale=(10.0, 10.0, 10.0),
            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.25, 0.0)),
        ),
        "robot_mesh": sim_utils.UsdFileCfg(
            usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/ANYbotics/ANYmal-C/anymal_c.usd",
            scale=(2.0, 2.0, 2.0),
            visual_material=sim_utils.GlassMdlCfg(glass_color=(0.0, 0.1, 0.0)),
        ),
    },
)

Drawing the markers#

To draw the markers, we call the visualize method. This method takes in as arguments the pose of the markers and the corresponding marker prototypes to draw.

while simulation_app.is_running():
    # rotate the markers around the z-axis for visualization
    marker_orientations = quat_from_angle_axis(yaw, torch.tensor([0.0, 0.0, 1.0]))
    # visualize
    my_visualizer.visualize(marker_locations, marker_orientations, marker_indices=marker_indices)
    # roll corresponding indices to show how marker prototype can be changed
    if yaw[0].item() % (0.5 * torch.pi) < 0.01:

Executing the Script#

To run the accompanying script, execute the following command:

./isaaclab.sh -p source/standalone/demos/markers.py

The simulation should start, and you can observe the different types of markers arranged in a grid pattern. The markers will rotating around their respective axes. Additionally every few rotations, they will roll forward on the grid.

To stop the simulation, close the window, or use Ctrl+C in the terminal.