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/camera/camera.py

@@ -13,13 +13,14 @@ import scipy.spatial.transform as tf
 from dataclasses import dataclass
 from typing import Any, Dict, Sequence, Tuple, Union
 
-import carb
 import omni.isaac.core.utils.prims as prim_utils
+import omni.isaac.core.utils.stage as stage_utils
 import omni.replicator.core as rep
 import omni.usd
-from omni.isaac.core.prims import XFormPrimView
+from omni.isaac.core.prims import XFormPrim
 from omni.isaac.core.simulation_context import SimulationContext
-from pxr import UsdGeom
+from omni.isaac.core.utils.rotations import gf_quat_to_np_array
+from pxr import Gf, Sdf, Usd, UsdGeom
 
 # omni-isaac-orbit
 from omni.isaac.orbit.utils import class_to_dict, to_camel_case
@@ -38,7 +39,7 @@ class CameraData:
     position: np.ndarray = None
     """Position of the sensor origin in world frame, following ROS convention."""
     orientation: np.ndarray = None
-    """Quaternion orientation `(w, x, y, z)` of the sensor origin in world frame, following ROS convention.."""
+    """Quaternion orientation `(w, x, y, z)` of the sensor origin in world frame, following ROS convention."""
     intrinsic_matrix: np.ndarray = None
     """The intrinsic matrix for the camera."""
     image_shape: Tuple[int, int] = None
@@ -55,6 +56,9 @@ class CameraData:
 class Camera(SensorBase):
     r"""The camera sensor for acquiring visual data.
 
+    This class wraps over the `UsdGeom Camera`_ for providing a consistent API for acquiring visual data.
+    It ensures that the camera follows the ROS convention for the coordinate system.
+
     Summarizing from the `replicator extension`_, the following sensor types are supported:
 
     - ``"rgb"``: A rendered color image.
@@ -78,15 +82,6 @@ class Camera(SensorBase):
     - ``"fisheye_polynomial"``: Fisheye camera model with :math:`360^{\circ}` spherical projection.
     - ``"fisheye_spherical"``: Fisheye camera model with :math:`360^{\circ}` full-frame projection.
 
-    Typically, the sensor comprises of two prims:
-
-    1. **Camera rig**: A dummy Xform prim to which the camera is attached to.
-    2. **Camera prim**: An instance of the `USDGeom Camera`_.
-
-    However, for the sake of generality, we allow omission of the camera rig prim. This is mostly the case when
-    the camera is static. In such cases, any request to set the camera pose is directly set on the camera prim,
-    instead of setting the pose of the camera rig Xform prim.
-
     .. _replicator extension: https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_replicator/annotators_details.html#annotator-output
     .. _USDGeom Camera: https://graphics.pixar.com/usd/docs/api/class_usd_geom_camera.html
 
@@ -111,10 +106,8 @@ class Camera(SensorBase):
         # TODO: Should this be done here or maybe inside the app config file?
         rep.settings.set_render_rtx_realtime(antialiasing="FXAA")
 
-        # Acquire simulation context
-        self._sim_context = SimulationContext.instance()
-        # Xform prim for the camera rig
-        self._sensor_rig_prim: XFormPrimView = None
+        # Xform prim for handling transforms
+        self._sensor_xform: XFormPrim = None
         # UsdGeom Camera prim for the sensor
         self._sensor_prim: UsdGeom.Camera = None
         # Create empty variables for storing output data
@@ -175,7 +168,7 @@ class Camera(SensorBase):
             visible (bool): Whether to make instance visible or invisible.
 
         Raises:
-            RuntimeError: If the camera prim is not set. Need to call `initialize(...)` first.
+            RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` first.
         """
         # check camera prim
         if self._sensor_prim is None:
@@ -191,6 +184,16 @@ class Camera(SensorBase):
     def set_intrinsic_matrix(self, matrix: np.ndarray, focal_length: float = 1.0):
         """Set parameters of the USD camera from its intrinsic matrix.
 
+        The intrinsic matrix and focal length are used to set the following parameters to the USD camera:
+
+        - ``focal_length``: The focal length of the camera.
+        - ``horizontal_aperture``: The horizontal aperture of the camera.
+        - ``vertical_aperture``: The vertical aperture of the camera.
+        - ``horizontal_aperture_offset``: The horizontal offset of the camera.
+        - ``vertical_aperture_offset``: The vertical offset of the camera.
+
+        .. warning::
+
             Due to limitations of Omniverse camera, we need to assume that the camera is a spherical lens,
             i.e. has square pixels, and the optical center is centered at the camera eye. If this assumption
             is not true in the input intrinsic matrix, then the camera will not set up correctly.
@@ -200,7 +203,7 @@ class Camera(SensorBase):
             focal_length (float, optional): Focal length to use when computing aperture values. Defaults to 1.0.
         """
         # convert to numpy for sanity
-        intrinsic_matrix = np.asarray(matrix).astype(np.float)
+        intrinsic_matrix = np.asarray(matrix, dtype=float)
         # extract parameters from matrix
         f_x = intrinsic_matrix[0, 0]
         c_x = intrinsic_matrix[0, 2]
@@ -233,10 +236,16 @@ class Camera(SensorBase):
     """
 
     def set_world_pose_ros(self, pos: Sequence[float] = None, quat: Sequence[float] = None):
-        """Set the pose of the camera w.r.t. world frame using ROS convention.
+        r"""Set the pose of the camera w.r.t. world frame using ROS convention.
 
-        This methods uses the ROS convention to resolve the input pose. In this convention,
-        we assume that the camera front-axis is +Z-axis and up-axis is -Y-axis.
+        In USD, the camera is always in **Y up** convention. This means that the camera is looking down the -Z axis
+        with the +Y axis pointing up , and +X axis pointing right. However, in ROS, the camera is looking down
+        the +Z axis with the +Y axis pointing down, and +X axis pointing right. Thus, the camera needs to be rotated
+        by :math:`180^{\circ}` around the X axis to follow the ROS convention.
+
+        .. math::
+
+            T_{ROS} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD}
 
         Args:
             pos (Sequence[float], optional): The cartesian coordinates (in meters). Defaults to None.
@@ -245,10 +254,6 @@ class Camera(SensorBase):
         Raises:
             RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first.
         """
-        # add note that this function is not working correctly
-        # FIXME: Fix this function. Getting the camera pose and setting back over here doesn't work.
-        carb.log_warn("The function `set_world_pose_ros` is currently not implemented correctly.")
-
         # check camera prim exists
         if self._sensor_prim is None:
             raise RuntimeError("Camera prim is None. Please call 'initialize(...)' first.")
@@ -260,95 +265,62 @@ class Camera(SensorBase):
             rotm = tf.Rotation.from_quat(convert_quat(quat, "xyzw")).as_matrix()
             rotm[:, 2] = -rotm[:, 2]
             rotm[:, 1] = -rotm[:, 1]
-            rotm = rotm.transpose()
-            quat_gl = tf.Rotation.from_matrix(rotm).as_quat()
             # convert to isaac-sim convention
+            quat_gl = tf.Rotation.from_matrix(rotm).as_quat()
             quat_gl = convert_quat(quat_gl, "wxyz")
         else:
             quat_gl = None
         # set the pose
-        if self._sensor_rig_prim is None:
-            # Note: Technically, we should prefer not to do this.
-            cam_prim = XFormPrimView(self.prim_path, reset_xform_properties=True)
-            cam_prim.set_world_poses(pos, quat_gl)
-        else:
-            self._sensor_rig_prim.set_world_poses(pos, quat_gl)
-
-    def set_world_pose_from_ypr(
-        self, target_position: Sequence[float], distance: float, yaw: float, pitch: float, roll: float, up_axis: str
-    ):
-        """Computes the view matrix from the inputs and sets the camera prim pose.
+        self._sensor_xform.set_world_pose(pos, quat_gl)
 
-        The implementation follows from the `computeViewMatrixFromYawPitchRoll()` function in Bullet3.
+    def set_world_pose_from_view(self, eye: Sequence[float], target: Sequence[float] = [0, 0, 0]):
+        """Set the pose of the camera from the eye position and look-at target position.
 
         Args:
-            target_position (Sequence[float]): Target focus point in cartesian world coordinates.
-            distance (float): Distance from eye to focus point.
-            yaw (float): Yaw angle in degrees (up, down).
-            pitch (float): Pitch angle in degrees around up vector.
-            roll (float): Roll angle in degrees around forward vector.
-            up_axis (str): The up axis for the camera. Either 'y', 'Y' or 'z', 'Z' axis.
+            eye (Sequence[float]): The position of the camera's eye.
+            target (Sequence[float], optional): The target location to look at. Defaults to [0, 0, 0].
 
         Raises:
             RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first.
-            ValueError: When the ``up_axis`` is not "y/Y" or "z/Z".
+            NotImplementedError: If the stage up-axis is not "Y" or "Z".
         """
-        # sanity conversion
-        camera_target_position = np.asarray(target_position)
-        up_axis = up_axis.upper()
         # check camera prim exists
         if self._sensor_prim is None:
             raise RuntimeError("Camera prim is None. Please call 'initialize(...)' first.")
-        # check inputs
-        if up_axis not in ["Y", "Z"]:
-            raise ValueError(f"Invalid specified up axis '{up_axis}'. Valid options: ['Y', 'Z'].")
         # compute camera's eye pose
-        if up_axis == "Y":
-            eye_position = np.array([0.0, 0.0, -distance])
-            eye_rotation = tf.Rotation.from_euler("ZYX", [roll, yaw, -pitch], degrees=True).as_matrix()
-            up_vector = np.array([0.0, 1.0, 0.0])
+        eye_position = Gf.Vec3d(np.asarray(eye).tolist())
+        target_position = Gf.Vec3d(np.asarray(target).tolist())
+        # compute forward direction
+        forward_dir = (eye_position - target_position).GetNormalized()
+        # get up axis
+        up_axis_token = stage_utils.get_stage_up_axis()
+        if up_axis_token == UsdGeom.Tokens.y:
+            # deal with degenerate case
+            if forward_dir == Gf.Vec3d(0, 1, 0):
+                up_axis = Gf.Vec3d(0, 0, 1)
+            elif forward_dir == Gf.Vec3d(0, -1, 0):
+                up_axis = Gf.Vec3d(0, 0, -1)
+            else:
+                up_axis = Gf.Vec3d(0, 1, 0)
+        elif up_axis_token == UsdGeom.Tokens.z:
+            # deal with degenerate case
+            if forward_dir == Gf.Vec3d(0, 0, 1):
+                up_axis = Gf.Vec3d(0, 1, 0)
+            elif forward_dir == Gf.Vec3d(0, 0, -1):
+                up_axis = Gf.Vec3d(0, -1, 0)
             else:
-            eye_position = np.array([0.0, -distance, 0.0])
-            eye_rotation = tf.Rotation.from_euler("ZYX", [yaw, roll, pitch], degrees=True).as_matrix()
-            up_vector = np.array([0.0, 0.0, 1.0])
-        # transform eye to get camera position
-        cam_up_vector = np.dot(eye_rotation, up_vector)
-        cam_position = np.dot(eye_rotation, eye_position) + camera_target_position
-        # axis direction for camera's view matrix
-        f = (camera_target_position - cam_position) / np.linalg.norm(camera_target_position - cam_position)
-        u = cam_up_vector / np.linalg.norm(cam_up_vector)
-        s = np.cross(f, u)
-        # create camera's view matrix: camera_T_world
-        cam_view_matrix = np.eye(4)
-        cam_view_matrix[:3, 0] = s
-        cam_view_matrix[:3, 1] = u
-        cam_view_matrix[:3, 2] = -f
-        cam_view_matrix[3, 0] = -np.dot(s, cam_position)
-        cam_view_matrix[3, 1] = -np.dot(u, cam_position)
-        cam_view_matrix[3, 2] = np.dot(f, cam_position)
-        # compute camera transform: world_T_camera
-        cam_tf = np.linalg.inv(cam_view_matrix)
-        cam_quat = tf.Rotation.from_matrix(cam_tf[:3, :3].T).as_quat()
-        cam_pos = cam_tf[3, :3]
+                up_axis = Gf.Vec3d(0, 0, 1)
+        else:
+            raise NotImplementedError(f"This method is not supported for up-axis '{up_axis_token}'.")
+        # compute matrix transformation
+        # view matrix: camera_T_world
+        matrix_gf = Gf.Matrix4d(1).SetLookAt(eye_position, target_position, up_axis)
+        # camera position and rotation in world frame
+        matrix_gf = matrix_gf.GetInverse()
+        cam_pos = np.array(matrix_gf.ExtractTranslation())
+        cam_quat = gf_quat_to_np_array(matrix_gf.ExtractRotationQuat())
         # set camera pose
-        self.set_camera_pose(cam_pos, cam_quat)
-
-    def set_world_pose_from_view(self, eye: Sequence[float], target: Sequence[float] = [0, 0, 0], vel: float = 0.0):
-        """Set the pose of the camera from the eye position and look-at target position.
-
-        Warn:
-            This method directly sets the camera prim pose and not the pose of the camera rig.
-            It is advised not to use it when the camera is part of a sensor rig.
-
-        Args:
-            eye (Sequence[float]): The position of the camera's eye.
-            target (Sequence[float], optional): The target location to look at. Defaults to [0, 0, 0].
-            vel (float, optional): The velocity of the camera.. Defaults to 0.0.
-        """
-        with self._rep_camera:
-            rep.modify.pose(position=eye, look_at=target)
-        # FIXME: add note that this function is not working correctly
-        carb.log_warn("The function `set_world_pose_from_view` is currently not implemented correctly.")
+        self._sensor_xform.set_world_pose(cam_pos, cam_quat)
 
     """
     Operations
@@ -357,48 +329,39 @@ class Camera(SensorBase):
     def spawn(self, parent_prim_path: str, translation: Sequence[float] = None, orientation: Sequence[float] = None):
         """Spawns the sensor into the stage.
 
-        The sensor is spawned under the parent prim at the path ``parent_prim_path`` with the provided input
-        rotation and translation. The USD Camera prim is attached to the parent prim.
+        The USD Camera prim is spawned under the parent prim at the path ``parent_prim_path`` with the provided input
+        translation and orientation.
 
         Args:
             parent_prim_path (str): The path of the parent prim to attach sensor to.
             translation (Sequence[float], optional): The local position offset w.r.t. parent prim. Defaults to None.
-            orientation (Sequence[float], optional): The local rotation offset in `(w, x, y, z)` w.r.t. parent prim. Defaults to None.
+            orientation (Sequence[float], optional): The local rotation offset in ``(w, x, y, z)`` w.r.t.
+                parent prim. Defaults to None.
         """
-        # Convert to camel case
-        projection_type = to_camel_case(self.cfg.projection_type, to="cC")
-        # Create camera using replicator. This creates under it two prims:
-        # 1) the rig: at the path f"{prim_path}/Camera_Xform"
-        # 2) the USD camera: at the path f"{prim_path}/Camera_Xform/Camera"
-        self._rep_camera = rep.create.camera(
-            parent=parent_prim_path,
-            projection_type=projection_type,
-            **class_to_dict(self.cfg.usd_params),
-        )
-        # Acquire the sensor prims
-        # 1) the rig
-        cam_rig_prim_path = rep.utils.get_node_targets(self._rep_camera.node, "inputs:prims")[0]
-        self._sensor_rig_prim = XFormPrimView(cam_rig_prim_path, reset_xform_properties=False)
-        # 2) the USD camera
-        cam_prim = prim_utils.get_prim_children(prim_utils.get_prim_at_path(cam_rig_prim_path))[0]
-        self._sensor_prim = UsdGeom.Camera(cam_prim)
+        # Check if sensor is already spawned
+        if self._is_spawned:
+            raise RuntimeError(f"The camera sensor instance has already been spawned at: {self.prim_path}.")
+        # Create sensor prim path
+        prim_path = stage_utils.get_next_free_path(f"{parent_prim_path}/Camera")
+        # Create sensor prim
+        self._sensor_prim = UsdGeom.Camera(prim_utils.define_prim(prim_path, "Camera"))
+        # Add replicator camera attributes
+        self._define_usd_camera_attributes()
         # Set the transformation of the camera
-        # Note: As mentioned in Isaac Sim documentation, it is better to never transform to camera directly.
-        #   Hence, we only transform the camera rig.
-        self._sensor_rig_prim.set_local_poses(translation, orientation)
+        self._sensor_xform = XFormPrim(self.prim_path)
+        self._sensor_xform.set_local_pose(translation, orientation)
         # Set spawning to true
         self._is_spawned = True
 
-    def initialize(self, cam_prim_path: str = None, has_rig: bool = False):
+    def initialize(self, cam_prim_path: str = None):
         """Initializes the sensor handles and internal buffers.
 
         This function creates handles and registers the provided data types with the replicator registry to
         be able to access the data from the sensor. It also initializes the internal buffers to store the data.
 
         The function also allows initializing to a camera not spawned by using the :meth:`spawn` method.
-        For instance, connecting to the default viewport camera "/Omniverse_persp". In such cases, it is
-        the user's responsibility to ensure that the camera is valid and inform the sensor class whether
-        the camera is part of a rig or not.
+        For instance, cameras that already exist in the USD stage. In such cases, the USD settings present on
+        the camera prim is used instead of the settings passed in the configuration object.
 
         Args:
             cam_prim_path (str, optional): The prim path to existing camera. Defaults to None.
@@ -406,7 +369,7 @@ class Camera(SensorBase):
 
         Raises:
             RuntimeError: When input `cam_prim_path` is :obj:`None`, the method defaults to using the last
-                camera prim path set when calling the :meth:`spawn()` function. In case, the camera was not spawned
+                camera prim path set when calling the :meth:`spawn` function. In case, the camera was not spawned
                 and no valid `cam_prim_path` is provided, the function throws an error.
         """
         # Check that sensor has been spawned
@@ -414,14 +377,14 @@ class Camera(SensorBase):
             if not self._is_spawned:
                 raise RuntimeError("Initialize the camera failed! Please provide a valid argument for `prim_path`.")
         else:
-            # Force to set active camera to input prim path/
+            # Get prim at path
             cam_prim = prim_utils.get_prim_at_path(cam_prim_path)
+            # Check if prim is valid
+            if cam_prim.IsValid() is False:
+                raise RuntimeError(f"Initialize the camera failed! Invalid prim path: {cam_prim_path}.")
+            # Force to set active camera to input prim path
             self._sensor_prim = UsdGeom.Camera(cam_prim)
-            # Check rig
-            if has_rig:
-                self._sensor_rig_prim = XFormPrimView(cam_prim_path.rsplit("/", 1)[0], reset_xform_properties=True)
-            else:
-                self._sensor_rig_prim = None
+            self._sensor_xform = XFormPrim(cam_prim_path)
 
         # Enable synthetic data sensors
         self._render_product_path = rep.create.render_product(
@@ -453,8 +416,11 @@ class Camera(SensorBase):
         # When running in standalone mode, need to render a few times to fill all the buffers
         # FIXME: Check with simulation team to get rid of this. What if someone has render or other callbacks?
         if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False:
+            # get simulation context
+            sim_context = SimulationContext.instance()
+            # render a few times
             for _ in range(4):
-                self._sim_context.render()
+                sim_context.render()
 
     def reset(self):
         # reset the timestamp
@@ -476,11 +442,6 @@ class Camera(SensorBase):
             When running in standalone mode, the function renders the scene a few times to fill all the buffers.
             During this time, the physics simulation is paused. This is a known issue with Isaac Sim.
         """
-        # When running in standalone mode, need to render a few times to fill all the buffers
-        # FIXME: Check with simulation team to get rid of this. What if someone has render or other callbacks?
-        if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False:
-            for _ in range(4):
-                self._sim_context.render()
         # -- intrinsic matrix
         self._data.intrinsic_matrix = self._compute_intrinsic_matrix()
         # -- pose
@@ -495,6 +456,52 @@ class Camera(SensorBase):
     Private Helpers
     """
 
+    def _define_usd_camera_attributes(self):
+        """Creates and sets USD camera attributes.
+
+        This function creates additional attributes on the camera prim used by Replicator.
+        It also sets the default values for these attributes based on the camera configuration.
+        """
+        # camera attributes
+        # reference: omni.replicator.core.scripts.create.py: camera()
+        attribute_types = {
+            "cameraProjectionType": "token",
+            "fthetaWidth": "float",
+            "fthetaHeight": "float",
+            "fthetaCx": "float",
+            "fthetaCy": "float",
+            "fthetaMaxFov": "float",
+            "fthetaPolyA": "float",
+            "fthetaPolyB": "float",
+            "fthetaPolyC": "float",
+            "fthetaPolyD": "float",
+            "fthetaPolyE": "float",
+        }
+        # get camera prim
+        prim = prim_utils.get_prim_at_path(self.prim_path)
+        # create attributes
+        for attr_name, attr_type in attribute_types.items():
+            # check if attribute does not exist
+            if prim.GetAttribute(attr_name).Get() is None:
+                # create attribute based on type
+                if attr_type == "token":
+                    prim.CreateAttribute(attr_name, Sdf.ValueTypeNames.Token)
+                elif attr_type == "float":
+                    prim.CreateAttribute(attr_name, Sdf.ValueTypeNames.Float)
+        # set attribute values
+        # -- projection type
+        projection_type = to_camel_case(self.cfg.projection_type, to="cC")
+        prim.GetAttribute("cameraProjectionType").Set(projection_type)
+        # -- other attributes
+        for param_name, param_value in class_to_dict(self.cfg.usd_params).items():
+            # check if value is valid
+            if param_value is None:
+                continue
+            # convert to camel case (CC)
+            param = to_camel_case(param_name, to="cC")
+            # get attribute from the class
+            prim.GetAttribute(param).Set(param_value)
+
     def _compute_intrinsic_matrix(self) -> np.ndarray:
         """Compute camera's matrix of intrinsic parameters.
 
@@ -508,7 +515,7 @@ class Camera(SensorBase):
             np.ndarray: A 3 x 3 numpy array containing the intrinsic parameters for the camera.
 
         Raises:
-            RuntimeError: If the camera prim is not set. Need to call `initialize(...)` first.
+            RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` first.
         """
         # check camera prim exists
         if self._sensor_prim is None:
@@ -540,7 +547,7 @@ class Camera(SensorBase):
             Tuple[np.ndarray, np.ndarray]: A tuple of the position (in meters) and quaternion (w, x, y, z).
         """
         # get camera's location in world space
-        prim_tf = UsdGeom.Xformable(self._sensor_prim).ComputeLocalToWorldTransform(0.0)
+        prim_tf = self._sensor_prim.ComputeLocalToWorldTransform(Usd.TimeCode.Default())
         # GfVec datatypes are row vectors that post-multiply matrices to effect transformations,
         # which implies, for example, that it is the fourth row of a GfMatrix4d that specifies
         # the translation of the transformation. Thus, we take transpose here to make it post multiply.

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,6 +187,8 @@ def main():
             num_channels=4,
         )
 
+        # 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()