Fixes setting of camera pose using ROS convention (#9)

* removes camera rig from camera class
* moves height scanner to use xformprim
* updates play-camera to use set_world_pose_ros
* fixes method for setting camera pose from view
* adds tests for setting camera poses
* makes camera testing consistent
* adds args to play camera to draw points
* removes calls to sim.render() in camera buffer()

docs/index.rst

@@ -64,6 +64,7 @@ using the following BibTeX entry:
 
    source/refs/contributing
    source/refs/troubleshooting
+   source/refs/issues
    source/refs/changelog
    source/refs/roadmap
    source/refs/license

docs/source/refs/issues.rst

+Known issues
+============
+
+Blank initial frames from the camera
+------------------------------------
+
+When using the :class:`Camera` sensor in standalone scripts, the first few frames may be blank.
+This is a known issue with the simulator where it needs a few steps to load the material
+textures properly and fill up the render targets.
+
+A hack to work around this is to add the following after initializing the camera sensor and setting
+its pose:
+
+.. code-block:: python
+
+    from omni.isaac.core.simulation_context import SimulationContext
+
+    sim = SimulationContext.instance()
+
+    # note: the number of steps might vary depending on how complicated the scene is.
+    for _ in range(12):
+        sim.render()

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

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

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

 Changelog
 ---------
 
+0.2.2 (2023-01-27)
+~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed the :meth:`set_world_pose_ros` and :meth:`set_world_pose_from_view` in the :class:`Camera` class.
+
+Deprecated
+^^^^^^^^^^
+
+* Removed the :meth:`set_world_pose_from_ypr` method from the :class:`Camera` class.
+
+
 0.2.1 (2023-01-26)
 ~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/height_scanner/height_scanner.py

@@ -12,7 +12,7 @@ from typing import List, Sequence
 import omni
 import omni.isaac.core.utils.prims as prim_utils
 import omni.isaac.core.utils.stage as stage_utils
-from omni.isaac.core.prims import XFormPrimView
+from omni.isaac.core.prims import XFormPrim
 
 # omni-isaac-orbit
 from omni.isaac.orbit.utils.math import convert_quat
@@ -93,7 +93,7 @@ class HeightScanner(SensorBase):
         # Whether to visualize the scanner points. Defaults to False.
         self._visualize = False
         # Xform prim for the sensor rig
-        self._sensor_prim: XFormPrimView = None
+        self._sensor_xform: XFormPrim = None
         # Create empty variables for storing output data
         self._data = HeightScannerData()
 
@@ -117,7 +117,7 @@ class HeightScanner(SensorBase):
     @property
     def prim_path(self) -> str:
         """The path to the height-map sensor."""
-        return self._sensor_prim.prim_paths[0]
+        return self._sensor_xform.prim_path
 
     @property
     def data(self) -> HeightScannerData:
@@ -166,7 +166,7 @@ class HeightScanner(SensorBase):
         prim_path = stage_utils.get_next_free_path(f"{parent_prim_path}/HeightScan_Xform")
         # Create the sensor prim
         prim_utils.create_prim(prim_path, "XForm")
-        self._sensor_prim = XFormPrimView(prim_path, translations=np.expand_dims(self.cfg.offset, axis=0))
+        self._sensor_xform = XFormPrim(prim_path, translation=self.cfg.offset)
         # Create visualization marker
         # TODO: Move this inside the height-scan prim to make it cleaner?
         vis_prim_path = stage_utils.get_next_free_path("/World/Visuals/HeightScan")
@@ -179,7 +179,7 @@ class HeightScanner(SensorBase):
         if not self._is_spawned:
             raise RuntimeError("Height scanner sensor must be spawned first. Please call `spawn(...)`.")
         # Initialize Xform class
-        self._sensor_prim.initialize()
+        self._sensor_xform.initialize()
         # Acquire physx ray-casting interface
         self._physx_query_interface = omni.physx.get_physx_scene_query_interface()
         # Since height scanner is fictitious sensor, we have no schema config to set in this case.

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

@@ -32,6 +32,7 @@ import omni.isaac.core.utils.stage as stage_utils
 import omni.replicator.core as rep
 from omni.isaac.core.prims import RigidPrim
 from omni.isaac.core.simulation_context import SimulationContext
+from omni.isaac.core.utils.torch import set_seed
 from omni.isaac.core.utils.viewports import set_camera_view
 from pxr import Gf, UsdGeom
 
@@ -52,13 +53,15 @@ class TestCameraSensor(unittest.TestCase):
     def setUp(self) -> None:
         """Create a blank new stage for each test."""
         # Simulation time-step
-        self.dt = 0.1
+        self.dt = 0.01
         # Load kit helper
         self.sim = SimulationContext(
             stage_units_in_meters=1.0, physics_dt=self.dt, rendering_dt=self.dt, backend="numpy"
         )
         # Set camera view
-        set_camera_view(eye=[15.0, 15.0, 15.0], target=[0.0, 0.0, 0.0])
+        set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0])
+        # Fix random seed -- to generate same scene every time
+        set_seed(0)
         # Spawn things into stage
         self._populate_scene()
         # Wait for spawning
@@ -130,6 +133,8 @@ class TestCameraSensor(unittest.TestCase):
         self.sim.reset()
         # Initialize sensor
         camera.initialize("/OmniverseKit_Persp")
+        # Set camera pose
+        set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0])
 
         # Simulate physics
         for i in range(10):
@@ -140,6 +145,8 @@ class TestCameraSensor(unittest.TestCase):
             # Save images
             rep_writer.write(camera.data.output)
             # Check image data
+            # expect same frame number
+            self.assertEqual(i + 1, camera.frame)
             # expected camera image shape
             height_expected, width_expected = camera.image_shape
             # check that the camera image shape is correct
@@ -151,7 +158,7 @@ class TestCameraSensor(unittest.TestCase):
                 self.assertEqual(width, width_expected)
 
     def test_single_cam(self):
-        """Checks that the single camera gets created properly with a rig."""
+        """Checks that the single camera gets created properly."""
         # Create directory to dump results
         test_dir = os.path.dirname(os.path.abspath(__file__))
         output_dir = os.path.join(test_dir, "output", "camera", "single")
@@ -189,7 +196,7 @@ class TestCameraSensor(unittest.TestCase):
         camera.initialize()
         # Set camera position directly
         # Note: Not a recommended way but was feeling lazy to do it properly.
-        camera.set_world_pose_from_view(eye=[15.0, 15.0, 15.0], target=[0.0, 0.0, 0.0])
+        camera.set_world_pose_from_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0])
 
         # Simulate physics
         for i in range(4):
@@ -211,7 +218,7 @@ class TestCameraSensor(unittest.TestCase):
                 self.assertEqual(width, width_expected)
 
     def test_multiple_cam(self):
-        """Checks that the multiple cameras created properly with and without rig."""
+        """Checks that the multiple cameras created properly."""
         # Create camera instance
         # -- default viewport
         camera_def_cfg = PinholeCameraCfg(
@@ -252,7 +259,7 @@ class TestCameraSensor(unittest.TestCase):
         camera_2.initialize()
 
         # Simulate physics
-        for i in range(10):
+        for _ in range(10):
             # perform rendering
             self.sim.step()
             # update camera
@@ -303,14 +310,16 @@ class TestCameraSensor(unittest.TestCase):
             camera.update(self.dt)
             # Check that matrix is correct
             K = camera.data.intrinsic_matrix
-            # TODO: This is not correctly setting all values in the matrix.
+            # TODO: This is not correctly setting all values in the matrix since the
+            #       vertical aperture and aperture offsets are not being set correctly
+            #       This is a bug in the simulator.
             self.assertAlmostEqual(rs_intrinsic_matrix[0, 0], K[0, 0], 4)
             # self.assertAlmostEqual(rs_intrinsic_matrix[1, 1], K[1, 1], 4)
         # Display results
         print(f">>> Desired intrinsic matrix: \n{rs_intrinsic_matrix}")
         print(f">>> Current intrinsic matrix: \n{camera.data.intrinsic_matrix}")
 
-    def test_pose_ros(self):
+    def test_set_pose_ros(self):
         """Checks that the camera's set and retrieve methods work for pose in ROS convention."""
         # Create camera instance
         camera_cfg = PinholeCameraCfg(
@@ -332,7 +341,7 @@ class TestCameraSensor(unittest.TestCase):
         # Simulate physics
         for _ in range(10):
             # set camera pose randomly
-            camera_position = np.random.random() * 5.0
+            camera_position = np.random.random(3) * 5.0
             camera_orientation = convert_quat(tf.Rotation.random().as_quat(), "wxyz")
             camera.set_world_pose_ros(pos=camera_position, quat=camera_orientation)
             # perform rendering
@@ -340,7 +349,57 @@ class TestCameraSensor(unittest.TestCase):
             # update camera
             camera.update(self.dt)
             # Check that pose is correct
+            # -- position
+            np.testing.assert_almost_equal(camera.data.position, camera_position, 4)
+            # -- orientation
+            if np.sign(camera.data.orientation[0]) != np.sign(camera_orientation[0]):
+                camera_orientation *= -1
+            np.testing.assert_almost_equal(camera.data.orientation, camera_orientation, 4)
+
+    def test_set_pose_from_view(self):
+        """Checks that the camera's set method works for look-at pose."""
+        # Create camera instance
+        camera_cfg = PinholeCameraCfg(
+            sensor_tick=0,
+            height=240,
+            width=320,
+            data_types=["rgb", "distance_to_image_plane"],
+            usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)),
+        )
+        camera = Camera(cfg=camera_cfg, device="cpu")
+        # Note: the camera is spawned by default in the stage
+        camera.spawn("/World/CameraSensor")
+
+        # Play simulator
+        self.sim.reset()
+        # Initialize sensor
+        camera.initialize()
+
+        # Test look-at pose
+        # -- inputs
+        eye = np.array([2.5, 2.5, 2.5])
+        targets = [np.array([0.0, 0.0, 0.0]), np.array([2.5, 2.5, 0.0])]
+        # -- expected outputs
+        camera_position = eye.copy()
+        camera_orientations = [
+            np.array([-0.17591989, 0.33985114, 0.82047325, -0.42470819]),
+            np.array([0.0, 1.0, 0.0, 0.0]),
+        ]
+
+        # check that the camera pose is correct
+        for target, camera_orientation in zip(targets, camera_orientations):
+            # set camera pose
+            camera.set_world_pose_from_view(eye=eye, target=target)
+            # perform rendering
+            self.sim.step()
+            # update camera
+            camera.update(self.dt)
+            # Check that pose is correct
+            # -- position
             np.testing.assert_almost_equal(camera.data.position, camera_position, 4)
+            # # -- orientation
+            if np.sign(camera.data.orientation[0]) != np.sign(camera_orientation[0]):
+                camera_orientation *= -1
             np.testing.assert_almost_equal(camera.data.orientation, camera_orientation, 4)
 
     def test_throughput(self):
@@ -369,6 +428,8 @@ class TestCameraSensor(unittest.TestCase):
         self.sim.reset()
         # Initialize sensor
         camera.initialize()
+        # Set camera pose
+        camera.set_world_pose_from_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0])
 
         # Simulate physics
         for _ in range(5):
@@ -418,15 +479,15 @@ class TestCameraSensor(unittest.TestCase):
         # Random objects
         for i in range(8):
             # sample random position
-            position = np.random.rand(3) - np.asarray([0.5, 0.5, -1.0])
-            position *= np.asarray([15.0, 15.0, 5.0])
+            position = np.random.rand(3) - np.asarray([0.05, 0.05, -1.0])
+            position *= np.asarray([1.5, 1.5, 0.5])
             # create prim
             prim_type = random.choice(["Cube", "Sphere", "Cylinder"])
             _ = prim_utils.create_prim(
                 f"/World/Objects/Obj_{i:02d}",
                 prim_type,
                 translation=position,
-                scale=(2.5, 2.5, 2.5),
+                scale=(0.25, 0.25, 0.25),
                 semantic_label=prim_type,
             )
             # add rigid properties
@@ -440,4 +501,4 @@ class TestCameraSensor(unittest.TestCase):
 
 
 if __name__ == "__main__":
-    unittest.main()
+    unittest.main(verbosity=2)

source/standalone/demo/play_camera.py

@@ -19,7 +19,8 @@ from omni.isaac.kit import SimulationApp
 # add argparse arguments
 parser = argparse.ArgumentParser("Welcome to Orbit: Omniverse Robotics Environments!")
 parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.")
-parser.add_argument("--gpu", action="store_true", default=False, help="Use gpu for pointcloud unprojection.")
+parser.add_argument("--gpu", action="store_true", default=False, help="Use GPU device for camera rendering output.")
+parser.add_argument("--draw", action="store_true", default=False, help="Draw the obtained pointcloud on viewport.")
 args_cli = parser.parse_args()
 
 # launch omniverse app
@@ -104,7 +105,6 @@ Main
 def main():
     """Runs a camera sensor from orbit."""
 
-    device = "cuda" if args_cli.gpu else "cpu"
     # Load kit helper
     sim = SimulationContext(stage_units_in_meters=1.0, physics_dt=0.005, rendering_dt=0.005, backend="torch")
     # Set main camera
@@ -120,15 +120,14 @@ def main():
         height=480,
         width=640,
         data_types=["rgb", "distance_to_image_plane", "normals", "motion_vectors"],
-        usd_params=PinholeCameraCfg.UsdCameraCfg(clipping_range=(0.1, 1.0e5)),
+        usd_params=PinholeCameraCfg.UsdCameraCfg(
+            focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5)
+        ),
     )
-    camera = Camera(cfg=camera_cfg, device=device)
+    camera = Camera(cfg=camera_cfg, device="cuda" if args_cli.gpu else "cpu")
 
     # Spawn camera
     camera.spawn("/World/CameraSensor")
-    # Initialize camera
-    # note: For rendering based sensors, it is not necessary to initialize before playing the simulation.
-    camera.initialize()
 
     # Create replicator writer
     output_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "output", "camera")
@@ -136,8 +135,22 @@ def main():
 
     # Play simulator
     sim.play()
-    # Set pose
-    camera.set_world_pose_from_view(eye=[5.0, 5.0, 5.0], target=[0.0, 0.0, 0.0])
+    # Initialize camera
+    camera.initialize()
+
+    # Set pose: There are two ways to set the pose of the camera.
+    # -- Option-1: Set pose using view
+    # camera.set_world_pose_from_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 0.0])
+    # -- Option-2: Set pose using ROS
+    position = [2.5, 2.5, 2.5]
+    orientation = [-0.17591989, 0.33985114, 0.82047325, -0.42470819]
+    camera.set_world_pose_ros(position, orientation)
+
+    # Simulate for a few steps
+    # note: This is a workaround to ensure that the textures are loaded.
+    #   Check "Known Issues" section in the documentation for more details.
+    for _ in range(14):
+        sim.render()
 
     # Simulate physics
     while simulation_app.is_running():
@@ -174,17 +187,19 @@ def main():
             num_channels=4,
         )
 
-        # Convert to numpy for visualization
-        if not isinstance(pointcloud_w, np.ndarray):
-            pointcloud_w = pointcloud_w.cpu().numpy()
-        if not isinstance(pointcloud_rgb, np.ndarray):
-            pointcloud_rgb = pointcloud_rgb.cpu().numpy()
-        # Visualize the points
-        num_points = pointcloud_w.shape[0]
-        points_size = [1.25] * num_points
-        points_color = pointcloud_rgb
-        draw_interface.clear_points()
-        draw_interface.draw_points(pointcloud_w.tolist(), points_color, points_size)
+        # Draw pointcloud
+        if not args_cli.headless and args_cli.draw:
+            # Convert to numpy for visualization
+            if not isinstance(pointcloud_w, np.ndarray):
+                pointcloud_w = pointcloud_w.cpu().numpy()
+            if not isinstance(pointcloud_rgb, np.ndarray):
+                pointcloud_rgb = pointcloud_rgb.cpu().numpy()
+            # Visualize the points
+            num_points = pointcloud_w.shape[0]
+            points_size = [1.25] * num_points
+            points_color = pointcloud_rgb
+            draw_interface.clear_points()
+            draw_interface.draw_points(pointcloud_w.tolist(), points_color, points_size)
 
 
 if __name__ == "__main__":