Adds a generalized implementation of `PointInstancer` based markers (#45)

# Description

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

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

Previously, we were having separate markers for frames, spheres, and
height scanners, each of them having minute differences. This PR
includes a more general wrapping around the
[`USDGeom.PointInstancer`](https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html)
to configure markers through configclass object. Additionally, it moves
to faster
[`pxr.Vt`](https://docs.omniverse.nvidia.com/kit/docs/kit-manual/latest/api/pxr.html#module-pxr.Vt)
arrays instead of keeping a list of
[`pxr.Gf`](https://docs.omniverse.nvidia.com/kit/docs/kit-manual/latest/api/pxr.html#module-pxr.Gf)
arrays.

The old markers still exist for now until we move to v1.0 release. They
are present in the `omni.isaac.orbit.compat` module.

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

## Type of change

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

- New feature (non-breaking change which adds functionality)
- Breaking change (fix or feature that would cause existing
functionality to not work as expected)
- This change requires a documentation update

## Checklist

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

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

For example,
- [x] I have done this task
- [ ] I have not done this task
-->

source/extensions/omni.isaac.orbit/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.4.2"
+version = "0.4.3"
 
 # Description
 title = "ORBIT framework for Robot Learning"

source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst

 Changelog
 ---------
 
+0.4.3 (2023-06-28)
+~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added the :class:`omni.isaac.orbit.markers.PointInstancerMarker` class that wraps around
+  `UsdGeom.PointInstancer <https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html>`_
+  to directly work with torch and numpy arrays.
+
+Changed
+^^^^^^^
+
+* Moved the old markers in :mod:`omni.isaac.orbit.markers` to :mod:`omni.isaac.orbit.compat.markers`.
+* Modified the standalone scripts to use the :mod:`omni.isaac.orbit.compat.markers` module.
+
+
 0.4.2 (2023-06-28)
 ~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/compat/markers/__init__.py

+# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""
+This submodule provides marker utilities for simplifying creation of UI elements in the GUI.
+
+Currently, the module provides two classes:
+
+* :class:`StaticMarker` for creating a group of markers from a single USD file.
+* :class:`PointMarker` for creating a group of spheres.
+
+
+.. note::
+
+    For some simple usecases, it may be sufficient to use the debug drawing utilities from Isaac Sim.
+    The debug drawing API is available in the `omni.isaac.debug_drawing`_ module. It allows drawing of
+    points and splines efficiently on the UI.
+
+    .. _omni.isaac.debug_drawing: https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/ext_omni_isaac_debug_drawing.html
+
+"""
+
+from .point_marker import PointMarker
+from .static_marker import StaticMarker
+
+__all__ = ["StaticMarker", "PointMarker"]

source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/point_marker.py → source/extensions/omni.isaac.orbit/omni/isaac/orbit/compat/markers/point_marker.py

@@ -35,7 +35,7 @@ class PointMarker:
 
         .. code-block:: python
 
-            from omni.isaac.orbit.utils.markers import PointMarker
+            from omni.isaac.orbit.compat.markers import PointMarker
 
             # create a point marker
             marker = PointMarker("/World/Visuals/goal", 24, radius=0.2)

source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/static_marker.py → source/extensions/omni.isaac.orbit/omni/isaac/orbit/compat/markers/static_marker.py

@@ -31,7 +31,7 @@ class StaticMarker:
 
         .. code-block:: python
 
-            from omni.isaac.orbit.utils.markers import StaticMarker
+            from omni.isaac.orbit.compat.markers import StaticMarker
 
             # create a static marker
             marker = StaticMarker("/World/Visuals/frames", 24, scale=(0.5, 0.5, 0.5))

source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/__init__.py

@@ -6,15 +6,15 @@
 """
 This submodule provides marker utilities for simplifying creation of UI elements in the GUI.
 
-Currently, the module provides two classes:
+Currently, the module provides the following classes:
 
-* :class:`StaticMarker` for creating a group of markers from a single USD file.
-* :class:`PointMarker` for creating a group of spheres.
+* :class:`VisualizationMarkers` for creating a group of markers using `UsdGeom.PointInstancer
+  <https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html>`_.
 
 
 .. note::
 
-    For some simple usecases, it may be sufficient to use the debug drawing utilities from Isaac Sim.
+    For some simple use-cases, it may be sufficient to use the debug drawing utilities from Isaac Sim.
     The debug drawing API is available in the `omni.isaac.debug_drawing`_ module. It allows drawing of
     points and splines efficiently on the UI.
 
@@ -22,7 +22,6 @@ Currently, the module provides two classes:
 
 """
 
-from .point_marker import PointMarker
-from .static_marker import StaticMarker
+from .visualization_markers import VisualizationMarkers, VisualizationMarkersCfg
 
-__all__ = ["StaticMarker", "PointMarker"]
+__all__ = ["VisualizationMarkersCfg", "VisualizationMarkers"]

source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/config/__init__.py

+from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
+
+from ..visualization_markers import VisualizationMarkersCfg
+
+FRAME_MARKER_CFG = VisualizationMarkersCfg(
+    markers={
+        "frame": VisualizationMarkersCfg.FileMarkerCfg(
+            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/UIElements/frame_prim.usd",
+            scale=(0.5, 0.5, 0.5),
+        )
+    }
+)
+"""Configuration for the frame marker."""
+
+POSITION_GOAL_MARKER_CFG = VisualizationMarkersCfg(
+    markers={
+        "target_far": VisualizationMarkersCfg.MarkerCfg(
+            prim_type="Sphere",
+            color=(1.0, 0.0, 0.0),
+            attributes={"radius": 0.01},
+        ),
+        "target_near": VisualizationMarkersCfg.MarkerCfg(
+            prim_type="Sphere",
+            color=(0.0, 1.0, 0.0),
+            attributes={"radius": 0.01},
+        ),
+        "target_invisible": VisualizationMarkersCfg.MarkerCfg(
+            prim_type="Sphere",
+            color=(0.0, 0.0, 1.0),
+            attributes={"radius": 0.01},
+            visible=False,
+        ),
+    }
+)
+"""Configuration for the end-effector tracking marker."""
+
+HEIGHT_SCAN_MARKER_CFG = VisualizationMarkersCfg(
+    markers={
+        "hit": VisualizationMarkersCfg.MarkerCfg(
+            prim_type="Sphere",
+            color=(1.0, 0.0, 0.0),
+            attributes={"radius": 0.01},
+        ),
+    },
+)
+"""Configuration for the height scan marker."""

source/extensions/omni.isaac.orbit/omni/isaac/orbit/markers/visualization_markers.py

+# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""A class to coordinate groups of visual markers (such as spheres, frames or arrows)
+using `UsdGeom.PointInstancer`_ class.
+
+The class :class:`VisualizationMarkers` is used to create a group of visual markers and
+visualize them in the viewport. The markers are represented as :class:`UsdGeom.PointInstancer` prims
+in the USD stage. The markers are created as prototypes in the :class:`UsdGeom.PointInstancer` prim
+and are instanced in the :class:`UsdGeom.PointInstancer` prim. The markers can be visualized by
+passing the indices of the marker prototypes and their translations, orientations and scales.
+The marker prototypes can be configured with the :class:`VisualizationMarkersCfg` class.
+
+.. _UsdGeom.PointInstancer: https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html
+"""
+
+import numpy as np
+import torch
+from dataclasses import MISSING
+from typing import Any, Dict, List, Optional, Union
+
+import omni.isaac.core.utils.prims as prim_utils
+import omni.kit.commands
+from omni.isaac.core.materials import PreviewSurface
+from omni.isaac.core.prims import GeometryPrim
+from pxr import Gf, UsdGeom, Vt
+
+from omni.isaac.orbit.utils.assets import check_file_path
+from omni.isaac.orbit.utils.configclass import configclass
+
+
+@configclass
+class VisualizationMarkersCfg:
+    """A class to configure a :class:`VisualizationMarkers`."""
+
+    @configclass
+    class MarkerCfg:
+        """A class to configure a marker prototype prim."""
+
+        visible: bool = True
+        """The visibility of the marker. Defaults to True."""
+        prim_type: str = MISSING
+        """The prim type of the marker.
+
+        This can be any valid USD prim type, such as "Sphere" or "Cone".
+        """
+        scale: Optional[List[float]] = None
+        """The scale of the marker. Defaults to None."""
+        color: Optional[List[float]] = None
+        """The RGB color of the marker. Defaults to None.
+
+        If not :obj:`None`, the marker will be colored with the given color. The color is
+        applied to the material of the marker prim with a preview surface shader that has a
+        precedence over any existing material on the prim.
+        """
+        attributes: Optional[Dict[str, Any]] = None
+        """The attributes of the marker. Defaults to None."""
+
+    @configclass
+    class FileMarkerCfg(MarkerCfg):
+        """A class to configure a marker prototype prim from a USD file."""
+
+        prim_type: str = "Xform"
+        """The prim type of the marker. Defaults to "Xform"."""
+        usd_path: str = MISSING
+        """The path to the USD file of the marker."""
+
+    markers: Dict[str, MarkerCfg] = MISSING
+    """The dictionary of marker configurations.
+
+    The key is the name of the marker, and the value is the configuration of the marker.
+    The key is used to identify the marker in the class.
+    """
+
+
+class VisualizationMarkers:
+    """A class to coordinate groups of visual markers (loaded from USD).
+
+    This class allows visualization of different UI markers in the scene, such as points and frames.
+    The class wraps around the `UsdGeom.PointInstancer`_ for efficient handling of objects
+    in the stage via instancing the created marker prototype prims.
+
+    A marker prototype prim is a reusable template prim used for defining variations of objects
+    in the scene. For example, a sphere prim can be used as a marker prototype prim to create
+    multiple sphere prims in the scene at different locations. Thus, prototype prims are useful
+    for creating multiple instances of the same prim in the scene.
+
+    The class parses the configuration to create different the marker prototypes into the stage. Each marker
+    prototype prim is created as a child of the :class:`UsdGeom.PointInstancer` prim. The prim path for the
+    the marker prim is resolved using the key of the marker in the :attr:`VisualizationMarkersCfg.markers`
+    dictionary. The marker prototypes are created using the :meth:`omni.isaac.core.utils.create_prim`
+    function, and then then instanced using :class:`UsdGeom.PointInstancer` prim to allow creating multiple
+    instances of the marker prims.
+
+    Switching between different marker prototypes is possible by calling the :meth:`visualize` method with
+    the prototype indices corresponding to the marker prototype. The prototype indices are based on the order
+    in the :attr:`VisualizationMarkersCfg.markers` dictionary. For example, if the dictionary has two markers,
+    "marker1" and "marker2", then their prototype indices are 0 and 1 respectively. The prototype indices
+    can be passed as a list or array of integers.
+
+    Usage:
+        The following snippet shows how to create 24 sphere markers with a radius of 1.0 at random translations
+        within the range [-1.0, 1.0]. The first 12 markers will be colored red and the rest will be colored green.
+
+        .. code-block:: python
+
+            from omni.isaac.orbit.markers import VisualizationMarkersCfg, VisualizationMarkers
+
+            # Create the markers configuration
+            # This creates two marker prototypes, "marker1" and "marker2" which are spheres with a radius of 1.0.
+            # The color of "marker1" is red and the color of "marker2" is green.
+            cfg = VisualizationMarkersCfg(
+                markers={
+                    "marker1": VisualizationMarkersCfg.MarkerCfg(
+                        prim_type="Sphere",
+                        attributes={"radius": 1.0},
+                        color=(1.0, 0.0, 0.0),
+                    ),
+                    "marker2": VisualizationMarkersCfg.MarkerCfg(
+                        prim_type="Sphere",
+                        attributes={"radius": 1.0},
+                        color=(0.0, 1.0, 0.0),
+                }
+            )
+            # Create the markers instance
+            # This will create a UsdGeom.PointInstancer prim at the given path along with the marker prototypes.
+            marker = VisualizationMarkers("/World/Visuals/testMarkers", cfg)
+
+            # Set position of the marker
+            # -- randomly sample translations between -1.0 and 1.0
+            marker_translations = np.random.uniform(-1.0, 1.0, (24, 3))
+            # -- this will create 24 markers at the given translations
+            # note: the markers will all be `marker1` since the marker indices are not given
+            marker.visualize(translations=marker_translations)
+
+            # alter the markers based on their prototypes indices
+            # first 12 markers will be marker1 and the rest will be marker2
+            # 0 -> marker1, 1 -> marker2
+            marker_indices = [0] * 12 + [1] * 12
+            # this will change the marker prototypes at the given indices
+            # note: the translations of the markers will not be changed from the previous call
+            #  since the translations are not given.
+            marker.visualize(marker_indices=marker_indices)
+
+            # alter the markers based on their prototypes indices and translations
+            marker.visualize(marker_indices=marker_indices, translations=marker_translations)
+
+    .. _UsdGeom.PointInstancer: https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html
+
+    """
+
+    def __init__(self, prim_path: str, cfg: VisualizationMarkersCfg):
+        """Initialize the class.
+
+        When the class is initialized, the :class:`UsdGeom.PointInstancer` is created into the stage
+        and the marker prims are registered into it.
+
+        Args:
+            prim_path (str): The prim path where the PointInstancer will be created.
+            cfg (VisualizationMarkersCfg): The configuration for the markers.
+
+        Raises:
+            ValueError: When a prim already exists at the :obj:`prim_path` and it is not a :class:`UsdGeom.PointInstancer`.
+            ValueError: When no markers are provided in the :obj:`cfg`.
+        """
+        # resolve default markers in the UI elements
+        # -- prim path
+        if prim_utils.is_prim_path_valid(prim_path):
+            # retrieve prim if it exists
+            prim = prim_utils.get_prim_at_path(prim_path)
+            if not prim.IsA(UsdGeom.PointInstancer):
+                raise ValueError(f"The prim at path {prim_path} cannot be parsed as a `UsdGeom.PointInstancer` object.")
+            self._instancer_manager = UsdGeom.PointInstancer(prim)
+        else:
+            # create a new prim
+            prim = prim_utils.define_prim(prim_path, "PointInstancer")
+            self._instancer_manager = UsdGeom.PointInstancer(prim)
+        # store inputs
+        self.prim_path = prim_path
+        self.cfg = cfg
+        # check if any markers is provided
+        if len(self.cfg.markers) == 0:
+            raise ValueError(f"The `cfg.markers` cannot be empty. Received: {self.cfg.markers}")
+
+        # create a child prim for the marker
+        self._add_markers_prototypes(self.cfg.markers)
+        # Note: We need to do this the first time to initialize the instancer.
+        #   Otherwise, the instancer will not be visible.
+        self._instancer_manager.GetProtoIndicesAttr().Set([0])
+        self._instancer_manager.GetPositionsAttr().Set([Gf.Vec3f()])
+        self._count = 1
+
+    def __str__(self):
+        """Return a string representation of the class.
+
+        Returns:
+            str: A string representation of the class.
+        """
+        msg = f"VisualizationMarkers(prim_path={self.prim_path})"
+        msg += f"\n\tCount: {self.count}"
+        msg += f"\n\tNumber of prototypes: {self.num_prototypes}"
+        msg += "\n\tMarkers Prototypes:"
+        for index, (name, marker) in enumerate(self.cfg.markers.items()):
+            msg += f"\n\t\t[Index: {index}]: {name}: {marker.to_dict()}"
+        return msg
+
+    """
+    Properties.
+    """
+
+    @property
+    def num_prototypes(self) -> int:
+        """The number of marker prototypes available."""
+        return len(self.cfg.markers)
+
+    @property
+    def count(self) -> int:
+        """The total number of marker instances."""
+        # TODO: Update this when the USD API is available (Isaac Sim 2023.1)
+        # return self._instancer_manager.GetInstanceCount()
+        return self._count
+
+    """
+    Operations.
+    """
+
+    def set_visibility(self, visible: bool):
+        """Sets the visibility of the markers.
+
+        The method does this through the USD API.
+
+        Args:
+            visible (bool): flag to set the visibility.
+        """
+        imageable = UsdGeom.Imageable(self._instancer_manager)
+        if visible:
+            imageable.MakeVisible()
+        else:
+            imageable.MakeInvisible()
+
+    def is_visible(self) -> bool:
+        """Checks the visibility of the markers.
+
+        Returns:
+            bool: True if the markers are visible, False otherwise.
+        """
+        return self._instancer_manager.GetVisibilityAttr().Get() != UsdGeom.Tokens.invisible
+
+    def visualize(
+        self,
+        translations: Optional[Union[np.ndarray, torch.Tensor]] = None,
+        orientations: Optional[Union[np.ndarray, torch.Tensor]] = None,
+        scales: Optional[Union[np.ndarray, torch.Tensor]] = None,
+        marker_indices: Optional[Union[List[int], np.ndarray, torch.Tensor]] = None,
+    ):
+        """Update markers in the viewport.
+
+        .. note::
+            If the prim `PointInstancer` is hidden in the stage, the function will simply return
+            without updating the markers. This helps in unnecessary computation when the markers
+            are not visible.
+
+        Whenever updating the markers, the input arrays must have the same number of elements
+        in the first dimension. If the number of elements is different, the `UsdGeom.PointInstancer`
+        will raise an error complaining about the mismatch.
+
+        Additionally, the function supports dynamic update of the markers. This means that the
+        number of markers can change between calls. For example, if you have 24 points that you
+        want to visualize, you can pass 24 translations, orientations, and scales. If you want to
+        visualize only 12 points, you can pass 12 translations, orientations, and scales. The
+        function will automatically update the number of markers in the scene.
+
+        The function will also update the marker prototypes based on their prototype indices. For instance,
+        if you have two marker prototypes, and you pass the following marker indices: [0, 1, 0, 1], the function
+        will update the first and third markers with the first prototype, and the second and fourth markers
+        with the second prototype. This is useful when you want to visualize different markers in the same
+        scene. The list of marker indices must have the same number of elements as the translations, orientations,
+        or scales. If the number of elements is different, the function will raise an error.
+
+        .. caution::
+            This function will update all the markers instanced from the prototypes. That means
+            if you have 24 markers, you will need to pass 24 translations, orientations, and scales.
+
+            If you want to update only a subset of the markers, you will need to handle the indices
+            yourself and pass the complete arrays to this function.
+
+        Args:
+            translations (Optional[Union[np.ndarray, torch.Tensor]], optional):
+                translations w.r.t. parent prim frame. Shape: (M, 3).
+                Defaults to :obj:`None`, which means left unchanged.
+            orientations (Optional[Union[np.ndarray, torch.Tensor]], optional):
+                Quaternion orientations (w, x, y, z) w.r.t. parent prim frame. Shape: (M, 4).
+                Defaults to :obj:`None`, which means left unchanged.
+            scales (Union[np.ndarray, torch.Tensor]): Scale applied before any rotation is applied. Shape: (M, 3).
+                Defaults to :obj:`None`, which means left unchanged.
+            marker_indices (Optional[Union[np.ndarray, torch.Tensor]], optional):
+                Decides which marker prototype to visualize. Shape: (M).
+                Defaults to :obj:`None`, which means left unchanged provided that the total number of markers
+                is the same as the previous call. If the number of markers is different, the function
+                will update the number of markers in the scene.
+
+        Raises:
+            ValueError: When input arrays do not follow the expected shapes.
+            ValueError: When the function is called with all :obj:`None` arguments.
+        """
+        # check if it is visible (if not then let's not waste time)
+        if not self.is_visible():
+            return
+        # check if we have any markers to visualize
+        num_markers = 0
+        # resolve inputs
+        # -- position
+        if translations is not None:
+            if isinstance(translations, torch.Tensor):
+                translations = translations.detach().cpu().numpy()
+            # check that shape is correct
+            if translations.shape[1] != 3 or len(translations.shape) != 2:
+                raise ValueError(f"Expected `translations` to have shape (M, 3). Received: {translations.shape}.")
+            # apply translations
+            self._instancer_manager.GetPositionsAttr().Set(Vt.Vec3fArray.FromNumpy(translations))
+            # update number of markers
+            num_markers = translations.shape[0]
+        # -- orientation
+        if orientations is not None:
+            if isinstance(orientations, torch.Tensor):
+                orientations = orientations.detach().cpu().numpy()
+            # check that shape is correct
+            if orientations.shape[1] != 4 or len(orientations.shape) != 2:
+                raise ValueError(f"Expected `orientations` to have shape (M, 4). Received: {orientations.shape}.")
+            # apply orientations
+            self._instancer_manager.GetOrientationsAttr().Set(Vt.QuathArray.FromNumpy(orientations))
+            # update number of markers
+            num_markers = orientations.shape[0]
+        # -- scales
+        if scales is not None:
+            if isinstance(scales, torch.Tensor):
+                scales = scales.detach().cpu().numpy()
+            # check that shape is correct
+            if scales.shape[1] != 3 or len(scales.shape) != 2:
+                raise ValueError(f"Expected `scales` to have shape (M, 3). Received: {scales.shape}.")
+            # apply scales
+            self._instancer_manager.GetScalesAttr().Set(Vt.Vec3fArray.FromNumpy(scales))
+            # update number of markers
+            num_markers = scales.shape[0]
+        # -- status
+        if marker_indices is not None or num_markers != self._count:
+            # apply marker indices
+            if marker_indices is not None:
+                if isinstance(marker_indices, torch.Tensor):
+                    marker_indices = marker_indices.detach().cpu().numpy()
+                elif isinstance(marker_indices, list):
+                    marker_indices = np.array(marker_indices)
+                # check that shape is correct
+                if len(marker_indices.shape) != 1:
+                    raise ValueError(f"Expected `marker_indices` to have shape (M,). Received: {marker_indices.shape}.")
+                # apply proto indices
+                self._instancer_manager.GetProtoIndicesAttr().Set(Vt.IntArray.FromNumpy(marker_indices))
+                # update number of markers
+                num_markers = marker_indices.shape[0]
+            else:
+                # check that number of markers is not zero
+                if num_markers == 0:
+                    raise ValueError("Number of markers cannot be zero! Hint: The function was called with no inputs?")
+                # set all markers to be the first prototype
+                self._instancer_manager.GetProtoIndicesAttr().Set([0] * num_markers)
+        # set number of markers
+        self._count = num_markers
+
+    """
+    Helper functions.
+    """
+
+    def _add_markers_prototypes(self, markers_cfg: Dict[str, VisualizationMarkersCfg.MarkerCfg]):
+        """Adds markers prototypes to the scene and sets the markers instancer to use them."""
+        # add markers based on config
+        for name, cfg in markers_cfg.items():
+            # handle basic marker config
+            # if it is a file marker, check if the file exists
+            # if it is a prim marker, check that prim type is not Xform since that is an empty prim!
+            if not isinstance(cfg, VisualizationMarkersCfg.FileMarkerCfg):
+                # check if prim type is valid
+                if cfg.prim_type == "Xform":
+                    raise ValueError("Please use `FileMarkerCfg` for `prim_type` of `Xform`.")
+                # set usd path as None
+                usd_path = None
+            else:
+                # check if the file exists
+                if not check_file_path(cfg.usd_path):
+                    raise FileNotFoundError(f"USD file for the marker not found at: {cfg.usd_path}")
+                # make sure user doesn't override the prim type
+                if cfg.prim_type != "Xform":
+                    raise ValueError(
+                        f"Please use `prim_type` of `Xform` for `FileMarkerCfg`. Received: {cfg.prim_type}."
+                    )
+                # set usd path
+                usd_path = cfg.usd_path
+            # resolve prim path
+            marker_prim_path = f"{self.prim_path}/{name}"
+            # create a child prim for the marker
+            prim = prim_utils.create_prim(
+                prim_path=marker_prim_path,
+                prim_type=cfg.prim_type,
+                usd_path=usd_path,
+                scale=cfg.scale,
+                attributes=cfg.attributes,
+            )
+            # remove any physics parameters
+            omni.kit.commands.execute("RemovePhysicsComponentCommand", usd_prim=prim, component="PhysicsRigidBodyAPI")
+            omni.kit.commands.execute("RemovePhysicsComponentCommand", usd_prim=prim, component="PhysicsCollisionAPI")
+            # set visibility
+            prim_utils.set_prim_visibility(prim, visible=cfg.visible)
+            # create color attribute
+            if cfg.color is not None:
+                geom_prim = GeometryPrim(marker_prim_path)
+                material = PreviewSurface(f"{marker_prim_path}/material", color=np.asarray(cfg.color))
+                geom_prim.apply_visual_material(material, weaker_than_descendants=False)
+            # add child reference to point instancer
+            self._instancer_manager.GetPrototypesRel().AddTarget(marker_prim_path)

source/extensions/omni.isaac.orbit/omni/isaac/orbit/terrains/terrain_importer.py

@@ -13,7 +13,7 @@ import omni.isaac.core.utils.prims as prim_utils
 import warp
 from pxr import UsdGeom
 
-from omni.isaac.orbit.markers import StaticMarker
+from omni.isaac.orbit.compat.markers import StaticMarker
 from omni.isaac.orbit.utils.kit import create_ground_plane
 
 from .terrain_cfg import TerrainImporterCfg

source/extensions/omni.isaac.orbit/test/markers/test_visualization_markers.py

+# Copyright [2023] Boston Dynamics AI Institute, Inc.
+# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+
+"""Launch Isaac Sim Simulator first."""
+
+from omni.isaac.kit import SimulationApp
+
+# launch omniverse app
+config = {"headless": True}
+simulation_app = SimulationApp(config)
+
+"""Rest everything follows."""
+
+import torch
+import traceback
+import unittest
+
+import carb
+from omni.isaac.core.simulation_context import SimulationContext
+
+from omni.isaac.orbit.markers import VisualizationMarkers, VisualizationMarkersCfg
+from omni.isaac.orbit.markers.config import FRAME_MARKER_CFG, POSITION_GOAL_MARKER_CFG
+from omni.isaac.orbit.utils.math import random_orientation
+from omni.isaac.orbit.utils.timer import Timer
+
+
+class TestUsdVisualizationMarkers(unittest.TestCase):
+    """Test fixture for the UrdfLoader class."""
+
+    def setUp(self):
+        """Create a blank new stage for each test."""
+        # Simulation time-step
+        self.dt = 0.01
+        # Load kit helper
+        self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="torch", device="cuda:0")
+
+    def tearDown(self) -> None:
+        """Stops simulator after each test."""
+        # stop simulation
+        self.sim.stop()
+        self.sim.clear()
+
+    def test_instantiation(self):
+        """Test that the class can be initialized properly."""
+        config = VisualizationMarkersCfg(
+            markers={
+                "test": VisualizationMarkersCfg.MarkerCfg(
+                    prim_type="Sphere",
+                    attributes={"radius": 1.0},
+                )
+            }
+        )
+        test_marker = VisualizationMarkers("/World/Visuals/test", config)
+        print(test_marker)
+        # check number of markers
+        self.assertEqual(test_marker.num_prototypes, 1)
+
+    def test_usd_cpu_marker(self):
+        """Test with marker from a USD."""
+        # create a marker
+        test_marker = VisualizationMarkers("/World/Visuals/test_frames", FRAME_MARKER_CFG)
+
+        # play the simulation
+        self.sim.reset()
+        # create a buffer
+        num_frames = 0
+        # run with randomization of poses
+        for count in range(1000):
+            # sample random poses
+            if count % 50 == 0:
+                num_frames = torch.randint(10, 1000, (1,)).item()
+                frame_translations = torch.randn((num_frames, 3))
+                frame_rotations = random_orientation(num_frames, device=self.sim.device)
+                # set the marker
+                test_marker.visualize(translations=frame_translations, orientations=frame_rotations)
+            # update the kit
+            self.sim.step()
+            # asset that count is correct
+            self.assertEqual(test_marker.count, num_frames)
+
+    def test_usd_gpu_marker(self):
+        """Test with marker from a USD."""
+        # create a marker
+        test_marker = VisualizationMarkers("/World/Visuals/test_frames", FRAME_MARKER_CFG)
+
+        # play the simulation
+        self.sim.reset()
+        # create a buffer
+        num_frames = 0
+        # run with randomization of poses
+        for count in range(1000):
+            # sample random poses
+            if count % 50 == 0:
+                num_frames = torch.randint(10, 1000, (1,)).item()
+                frame_translations = torch.randn((num_frames, 3), device=self.sim.device)
+                frame_rotations = random_orientation(num_frames, device=self.sim.device)
+                # set the marker
+                test_marker.visualize(translations=frame_translations, orientations=frame_rotations)
+            # update the kit
+            self.sim.step()
+            # asset that count is correct
+            self.assertEqual(test_marker.count, num_frames)
+
+    def test_usd_marker_color(self):
+        """Test with marker from a USD with its color modified."""
+        # create a marker
+        FRAME_MARKER_CFG.markers["frame"].color = (0.0, 1.0, 0.0)
+        test_marker = VisualizationMarkers("/World/Visuals/test_frames", FRAME_MARKER_CFG)
+
+        # play the simulation
+        self.sim.reset()
+        # run with randomization of poses
+        for count in range(1000):
+            # sample random poses
+            if count % 50 == 0:
+                num_frames = torch.randint(10, 1000, (1,)).item()
+                frame_translations = torch.randn((num_frames, 3))
+                frame_rotations = random_orientation(num_frames, device=self.sim.device)
+                # set the marker
+                test_marker.visualize(translations=frame_translations, orientations=frame_rotations)
+            # update the kit
+            self.sim.step()
+
+    def test_multiple_prototypes_marker(self):
+        """Test with multiple prototypes of spheres."""
+        # create a marker
+        test_marker = VisualizationMarkers("/World/Visuals/test_protos", POSITION_GOAL_MARKER_CFG)
+
+        # play the simulation
+        self.sim.reset()
+        # run with randomization of poses
+        for count in range(1000):
+            # sample random poses
+            if count % 50 == 0:
+                num_frames = torch.randint(100, 1000, (1,)).item()
+                frame_translations = torch.randn((num_frames, 3))
+                # randomly choose a prototype
+                marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,))
+                # set the marker
+                test_marker.visualize(translations=frame_translations, marker_indices=marker_indices)
+            # update the kit
+            self.sim.step()
+
+    def test_visualization_time_based_on_prototypes(self):
+        """Test with time taken when number of prototypes is increased."""
+
+        # create a marker
+        test_marker = VisualizationMarkers("/World/Visuals/test_protos", POSITION_GOAL_MARKER_CFG)
+
+        # play the simulation
+        self.sim.reset()
+        # number of frames
+        num_frames = 4096
+
+        # check that visibility is true
+        self.assertTrue(test_marker.is_visible())
+        # run with randomization of poses and indices
+        frame_translations = torch.randn((num_frames, 3))
+        marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,))
+        # set the marker
+        with Timer("Marker visualization with explicit indices") as timer:
+            test_marker.visualize(translations=frame_translations, marker_indices=marker_indices)
+            # save the time
+            time_with_marker_indices = timer.time_elapsed
+
+        with Timer("Marker visualization with no indices") as timer:
+            test_marker.visualize(translations=frame_translations)
+            # save the time
+            time_with_no_marker_indices = timer.time_elapsed
+
+        # update the kit
+        self.sim.step()
+        # check that the time is less
+        self.assertLess(time_with_no_marker_indices, time_with_marker_indices)
+
+    def test_visualization_time_based_on_visibility(self):
+        """Test with visibility of markers. When invisible, the visualize call should return."""
+
+        # create a marker
+        test_marker = VisualizationMarkers("/World/Visuals/test_protos", POSITION_GOAL_MARKER_CFG)
+
+        # play the simulation
+        self.sim.reset()
+        # number of frames
+        num_frames = 4096
+
+        # check that visibility is true
+        self.assertTrue(test_marker.is_visible())
+        # run with randomization of poses and indices
+        frame_translations = torch.randn((num_frames, 3))
+        marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,))
+        # set the marker
+        with Timer("Marker visualization") as timer:
+            test_marker.visualize(translations=frame_translations, marker_indices=marker_indices)
+            # save the time
+            time_with_visualization = timer.time_elapsed
+
+        # update the kit
+        self.sim.step()
+        # make invisible
+        test_marker.set_visibility(False)
+
+        # check that visibility is false
+        self.assertFalse(test_marker.is_visible())
+        # run with randomization of poses and indices
+        frame_translations = torch.randn((num_frames, 3))
+        marker_indices = torch.randint(0, test_marker.num_prototypes, (num_frames,))
+        # set the marker
+        with Timer("Marker no visualization") as timer:
+            test_marker.visualize(translations=frame_translations, marker_indices=marker_indices)
+            # save the time
+            time_with_no_visualization = timer.time_elapsed
+
+        # check that the time is less
+        self.assertLess(time_with_no_visualization, time_with_visualization)
+
+
+if __name__ == "__main__":
+    try:
+        unittest.main()
+    except Exception as err:
+        carb.log_error(err)
+        carb.log_error(traceback.format_exc())
+        raise
+    finally:
+        # close sim app
+        simulation_app.close()

source/extensions/omni.isaac.orbit/test/test_timer.py

@@ -26,7 +26,7 @@ class TestTimer(unittest.TestCase):
         timer.stop()
         self.assertAlmostEqual(1, timer.total_run_time, self.precision_places)
 
-    def test_tiemr_as_context_manager(self):
+    def test_timer_as_context_manager(self):
         """Test using a `Timer` as a context manager."""
         with Timer() as timer:
             self.assertAlmostEqual(0, timer.time_elapsed, self.precision_places)

source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/locomotion/velocity/velocity_env.py

@@ -15,7 +15,7 @@ import omni.replicator.isaac as rep_dr
 from omni.isaac.core.utils.types import DynamicsViewState
 
 import omni.isaac.orbit.utils.kit as kit_utils
-from omni.isaac.orbit.markers import PointMarker, StaticMarker
+from omni.isaac.orbit.compat.markers import PointMarker, StaticMarker
 from omni.isaac.orbit.robots.legged_robot import LeggedRobot
 from omni.isaac.orbit.utils.dict import class_to_dict
 from omni.isaac.orbit.utils.math import quat_apply, quat_from_euler_xyz, sample_uniform, wrap_to_pi

source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/lift/lift_env.py

@@ -11,8 +11,8 @@ from typing import List
 import omni.isaac.core.utils.prims as prim_utils
 
 import omni.isaac.orbit.utils.kit as kit_utils
+from omni.isaac.orbit.compat.markers import StaticMarker
 from omni.isaac.orbit.controllers.differential_inverse_kinematics import DifferentialInverseKinematics
-from omni.isaac.orbit.markers import StaticMarker
 from omni.isaac.orbit.objects import RigidObject
 from omni.isaac.orbit.robots.single_arm import SingleArmManipulator
 from omni.isaac.orbit.utils.dict import class_to_dict

source/extensions/omni.isaac.orbit_envs/omni/isaac/orbit_envs/manipulation/reach/reach_env.py

@@ -10,8 +10,8 @@ import torch
 import omni.isaac.core.utils.prims as prim_utils
 
 import omni.isaac.orbit.utils.kit as kit_utils
+from omni.isaac.orbit.compat.markers import PointMarker, StaticMarker
 from omni.isaac.orbit.controllers.differential_inverse_kinematics import DifferentialInverseKinematics
-from omni.isaac.orbit.markers import PointMarker, StaticMarker
 from omni.isaac.orbit.robots.single_arm import SingleArmManipulator
 from omni.isaac.orbit.utils.dict import class_to_dict
 from omni.isaac.orbit.utils.math import random_orientation, sample_uniform, scale_transform

source/standalone/demo/play_ik_control.py

@@ -39,11 +39,11 @@ from omni.isaac.core.utils.carb import set_carb_setting
 from omni.isaac.core.utils.viewports import set_camera_view
 
 import omni.isaac.orbit.utils.kit as kit_utils
+from omni.isaac.orbit.compat.markers import StaticMarker
 from omni.isaac.orbit.controllers.differential_inverse_kinematics import (
     DifferentialInverseKinematics,
     DifferentialInverseKinematicsCfg,
 )
-from omni.isaac.orbit.markers import StaticMarker
 from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
 from omni.isaac.orbit.robots.config.universal_robots import UR10_CFG
 from omni.isaac.orbit.robots.single_arm import SingleArmManipulator

source/standalone/demo/play_quadrupeds.py

@@ -42,7 +42,7 @@ from omni.isaac.core.simulation_context import SimulationContext
 from omni.isaac.core.utils.viewports import set_camera_view
 
 import omni.isaac.orbit.utils.kit as kit_utils
-from omni.isaac.orbit.markers import PointMarker, StaticMarker
+from omni.isaac.orbit.compat.markers import PointMarker, StaticMarker
 from omni.isaac.orbit.robots.config.anymal import ANYMAL_B_CFG, ANYMAL_C_CFG
 from omni.isaac.orbit.robots.config.unitree import UNITREE_A1_CFG
 from omni.isaac.orbit.robots.legged_robot import LeggedRobot

source/standalone/demo/play_rmpflow.py

@@ -39,9 +39,9 @@ from omni.isaac.core.utils.carb import set_carb_setting
 from omni.isaac.core.utils.viewports import set_camera_view
 
 import omni.isaac.orbit.utils.kit as kit_utils
+from omni.isaac.orbit.compat.markers import StaticMarker
 from omni.isaac.orbit.controllers.config.rmp_flow import FRANKA_RMPFLOW_CFG, UR10_RMPFLOW_CFG
 from omni.isaac.orbit.controllers.rmp_flow import RmpFlowController
-from omni.isaac.orbit.markers import StaticMarker
 from omni.isaac.orbit.robots.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
 from omni.isaac.orbit.robots.config.universal_robots import UR10_CFG
 from omni.isaac.orbit.robots.single_arm import SingleArmManipulator