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/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