OMPE-92490: Fix singular rotation matrix and non-rotation quaternion

source/isaaclab/changelog.d/jmart-singular-rotation.rst

@@ -0,0 +1,16 @@
+Fixed
+^^^^^
+
+* Fixed :func:`~isaaclab.utils.math.create_rotation_matrix_from_view` returning a singular
+  matrix when the look-at direction was parallel to the up axis. The function now produces
+  a valid orthonormal frame via an alternate reference vector, and fills NaN for rows with
+  truly undefined forward direction (``eyes == targets`` or non-finite input). Callers
+  detect per-row failure with ``torch.isnan(R).any(dim=(-2, -1))``.
+* Fixed :func:`~isaaclab.utils.math.quat_from_matrix` silently returning a non-unit
+  quaternion for non-rotation input (singular, reflection, or scale-error matrices).
+  Such inputs now return NaN, detectable via :func:`torch.isnan`.
+* Fixed :meth:`~isaaclab.sensors.camera.Camera.set_world_poses_from_view` and
+  :meth:`~isaaclab.sensors.ray_caster.RayCasterCamera.set_world_poses_from_view` silently
+  applying garbage poses when an eye position equaled its target. Degenerate rows are now
+  skipped (with a logged warning), and ``ValueError`` is raised if every row in the batch
+  is degenerate.

source/isaaclab/isaaclab/sensors/camera/camera.py

@@ -314,16 +314,33 @@ def set_world_poses_from_view(
         Raises:
             RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first.
             NotImplementedError: If the stage up-axis is not "Y" or "Z".
+            ValueError: If every eye position equals its target (look-at direction undefined for the
+                whole batch). When only some rows are degenerate, those rows are skipped and the
+                remaining poses are still applied; a warning is logged.
         """
-        # resolve env_ids
+        # resolve env_ids to a tensor up front so we can index it during partial-failure filtering
         if env_ids is None:
             env_ids = self._ALL_INDICES
-        # get up axis of current stage
-        up_axis = UsdGeom.GetStageUpAxis(self.stage)
-        # set camera poses using the view
-        orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, up_axis, device=self._device))
         if not isinstance(env_ids, torch.Tensor):
             env_ids = torch.tensor(env_ids, dtype=torch.int32, device=self._device)
+        # get up axis of current stage
+        up_axis = UsdGeom.GetStageUpAxis(self.stage)
+        # set camera poses using the view; degenerate rows (eye == target) come back as NaN
+        rotation_matrix = create_rotation_matrix_from_view(eyes, targets, up_axis, device=self._device)
+        valid_indices = (~torch.isnan(rotation_matrix).any(dim=(-2, -1))).nonzero(as_tuple=True)[0]
+        n_valid = valid_indices.numel()
+        n_total = rotation_matrix.shape[0]
+        if n_valid == 0:
+            raise ValueError("look-at is undefined: every eye position equals its target")
+        if n_valid < n_total:
+            logger.warning(
+                "set_world_poses_from_view: skipping %d pose(s) where eye equals target",
+                n_total - n_valid,
+            )
+            rotation_matrix = rotation_matrix.index_select(0, valid_indices)
+            eyes = eyes.index_select(0, valid_indices)
+            env_ids = env_ids.index_select(0, valid_indices)
+        orientations = quat_from_matrix(rotation_matrix)
         idx_wp = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32)
         self._view.set_world_poses(wp.from_torch(eyes.contiguous()), wp.from_torch(orientations.contiguous()), idx_wp)
 

source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py

@@ -252,13 +252,35 @@ def set_world_poses_from_view(
         Raises:
             RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first.
             NotImplementedError: If the stage up-axis is not "Y" or "Z".
+            ValueError: If every eye position equals its target (look-at direction undefined for the
+                whole batch). When only some rows are degenerate, those rows are skipped and the
+                remaining poses are still applied; a warning is logged.
         """
+        # resolve env_ids to a tensor up front so we can index it during partial-failure filtering
+        if env_ids is None:
+            env_ids = self._ALL_INDICES
+        if not isinstance(env_ids, torch.Tensor):
+            env_ids = torch.tensor(env_ids, dtype=torch.long, device=self._device)
         # get up axis of current stage
         up_axis = UsdGeom.GetStageUpAxis(self.stage)
-        # camera position and rotation in opengl convention
-        orientations = math_utils.quat_from_matrix(
-            math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis=up_axis, device=self._device)
+        # camera position and rotation in opengl convention; degenerate rows (eye == target) come back as NaN
+        rotation_matrix = math_utils.create_rotation_matrix_from_view(
+            eyes, targets, up_axis=up_axis, device=self._device
         )
+        valid_indices = (~torch.isnan(rotation_matrix).any(dim=(-2, -1))).nonzero(as_tuple=True)[0]
+        n_valid = valid_indices.numel()
+        n_total = rotation_matrix.shape[0]
+        if n_valid == 0:
+            raise ValueError("look-at is undefined: every eye position equals its target")
+        if n_valid < n_total:
+            logger.warning(
+                "set_world_poses_from_view: skipping %d pose(s) where eye equals target",
+                n_total - n_valid,
+            )
+            rotation_matrix = rotation_matrix.index_select(0, valid_indices)
+            eyes = eyes.index_select(0, valid_indices)
+            env_ids = env_ids.index_select(0, valid_indices)
+        orientations = math_utils.quat_from_matrix(rotation_matrix)
         self.set_world_poses(eyes, orientations, env_ids, convention="opengl")
 
     """

source/isaaclab/isaaclab/utils/math.py

@@ -322,7 +322,9 @@ def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor:
         matrix: The rotation matrices. Shape is (..., 3, 3).
 
     Returns:
-        The quaternion in (x, y, z, w). Shape is (..., 4).
+        The quaternion in (x, y, z, w). Shape is (..., 4). Rows whose input is not a
+        valid rotation (e.g. singular, reflection, or scale-error matrices) are filled
+        with NaN, so callers can detect them via :func:`torch.isnan`.
 
     Reference:
         https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L102-L161
@@ -368,9 +370,13 @@ def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor:
 
     # if not for numerical problems, quat_candidates[i] should be same (up to a sign),
     # forall i; we pick the best-conditioned one (with the largest denominator)
-    return quat_candidates[torch.nn.functional.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5, :].reshape(
+    quat = quat_candidates[torch.nn.functional.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5, :].reshape(
         batch_dim + (4,)
     )
+    # guard against non-rotation input: a valid rotation must yield a unit quaternion.
+    # Threshold is 2x the worst-case float32 accumulated error (~1e-5) through this function.
+    invalid = (quat.norm(p=2, dim=-1, keepdim=True) - 1.0).abs() > 2e-5
+    return torch.where(invalid, torch.full_like(quat, float("nan")), quat)
 
 
 def _axis_angle_rotation(axis: Literal["X", "Y", "Z"], angle: torch.Tensor) -> torch.Tensor:
@@ -1633,7 +1639,17 @@ def create_rotation_matrix_from_view(
     The vectors are broadcast against each other so they all have shape (N, 3).
 
     Returns:
-        R: (N, 3, 3) batched rotation matrices
+        ``(N, 3, 3)`` batched rotation matrices. Rows with an undefined forward
+        direction (``eyes == targets`` or non-finite input) are filled with NaN.
+        Callers detect per-row failure with ``torch.isnan(R).any(dim=(-2, -1))``
+        and total failure with ``.all()``.
+
+    Note:
+        When the look-at direction is parallel to ``up_axis`` the camera roll
+        is mathematically undefined; a deterministic frame is returned via an
+        alternate reference vector. Tracking a target continuously through the
+        singularity will produce a discontinuous rotation -- smooth tracking
+        requires interpolation at the caller (e.g., quaternion slerp).
 
     Reference:
     Based on PyTorch3D (https://github.com/facebookresearch/pytorch3d/blob/eaf0709d6af0025fe94d1ee7cec454bc3054826a/pytorch3d/renderer/cameras.py#L1635-L1685)
@@ -1645,16 +1661,27 @@ def create_rotation_matrix_from_view(
     else:
         raise ValueError(f"Invalid up axis: {up_axis}. Valid options are 'Y' and 'Z'.")
 
+    forward = targets - eyes
+    # 1e-5 matches the torch.nn.functional.normalize eps below: smaller magnitudes produce a sub-unit z_axis
+    undefined_forward = (torch.linalg.norm(forward, dim=1, keepdim=True) < 1e-5) | ~torch.isfinite(forward).all(
+        dim=1, keepdim=True
+    )
+
     # get rotation matrix in opengl format (-Z forward, +Y up)
-    z_axis = -torch.nn.functional.normalize(targets - eyes, eps=1e-5)
+    z_axis = -torch.nn.functional.normalize(forward, eps=1e-5)
     x_axis = torch.nn.functional.normalize(torch.cross(up_axis_vec, z_axis, dim=1), eps=1e-5)
     y_axis = torch.nn.functional.normalize(torch.cross(z_axis, x_axis, dim=1), eps=1e-5)
     is_close = torch.isclose(x_axis, torch.tensor(0.0), atol=5e-3).all(dim=1, keepdim=True)
     if is_close.any():
-        replacement = torch.nn.functional.normalize(torch.cross(y_axis, z_axis, dim=1), eps=1e-5)
-        x_axis = torch.where(is_close, replacement, x_axis)
-    R = torch.cat((x_axis[:, None, :], y_axis[:, None, :], z_axis[:, None, :]), dim=1)
-    return R.transpose(1, 2)
+        # alt-up substitution when up_axis_vec is parallel to z_axis; both x and y must be recomputed.
+        # World X is non-parallel to z whenever the symptom fires for the supported up_axis values.
+        alt_up = torch.tensor((1.0, 0.0, 0.0), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1)
+        replacement_x = torch.nn.functional.normalize(torch.cross(alt_up, z_axis, dim=1), eps=1e-5)
+        replacement_y = torch.nn.functional.normalize(torch.cross(z_axis, replacement_x, dim=1), eps=1e-5)
+        x_axis = torch.where(is_close, replacement_x, x_axis)
+        y_axis = torch.where(is_close, replacement_y, y_axis)
+    R = torch.cat((x_axis[:, None, :], y_axis[:, None, :], z_axis[:, None, :]), dim=1).transpose(1, 2)
+    return torch.where(undefined_forward.unsqueeze(-1), torch.full_like(R, float("nan")), R)
 
 
 def make_pose(pos: torch.Tensor, rot: torch.Tensor) -> torch.Tensor:

source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py

@@ -752,11 +752,11 @@ def test_output_equal_to_usd_camera_when_intrinsics_set(setup_simulation):
 
     # set camera position
     camera_warp.set_world_poses_from_view(
-        eyes=torch.tensor([[0.1, 0.0, 5.0]], device=camera_warp.device),
+        eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_warp.device),
         targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_warp.device),
     )
     camera_usd.set_world_poses_from_view(
-        eyes=torch.tensor([[0.1, 0.0, 5.0]], device=camera_usd.device),
+        eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_usd.device),
         targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_usd.device),
     )
 

source/isaaclab/test/sensors/test_ray_caster_camera.py

@@ -898,11 +898,11 @@ def test_output_equal_to_usd_camera_when_intrinsics_set(setup_sim, focal_length_
 
     # set camera position
     camera_warp.set_world_poses_from_view(
-        eyes=torch.tensor([[0.1, 0.0, 5.0]], device=camera_warp.device),
+        eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_warp.device),
         targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_warp.device),
     )
     camera_usd.set_world_poses_from_view(
-        eyes=torch.tensor([[0.1, 0.0, 5.0]], device=camera_usd.device),
+        eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_usd.device),
         targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_usd.device),
     )
 

source/isaaclab/test/utils/test_math.py

@@ -1326,3 +1326,117 @@ def test_euler_xyz_from_quat():
         wrapped = expected % (2 * torch.pi)
         output = torch.stack(math_utils.euler_xyz_from_quat(quat, wrap_to_2pi=True), dim=-1)
         torch.testing.assert_close(output, wrapped)
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_create_rotation_matrix_from_view_lookat_along_up_axis_z(device):
+    """Camera above target on +Z axis with Z-up should return a valid orthonormal frame."""
+    eyes = torch.tensor([[0.0, 0.0, 5.0]], device=device)
+    targets = torch.tensor([[0.0, 0.0, 0.0]], device=device)
+    R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device)
+    assert R is not None
+    identity = torch.eye(3, device=device).expand(1, 3, 3)
+    torch.testing.assert_close(R @ R.transpose(-1, -2), identity, atol=1e-5, rtol=1e-5)
+    torch.testing.assert_close(torch.linalg.det(R), torch.ones(1, device=device), atol=1e-5, rtol=1e-5)
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_create_rotation_matrix_from_view_lookat_along_up_axis_y(device):
+    """Camera at +Y looking at origin with Y-up should return a valid orthonormal frame."""
+    eyes = torch.tensor([[0.0, 5.0, 0.0]], device=device)
+    targets = torch.tensor([[0.0, 0.0, 0.0]], device=device)
+    R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Y", device=device)
+    assert R is not None
+    identity = torch.eye(3, device=device).expand(1, 3, 3)
+    torch.testing.assert_close(R @ R.transpose(-1, -2), identity, atol=1e-5, rtol=1e-5)
+    torch.testing.assert_close(torch.linalg.det(R), torch.ones(1, device=device), atol=1e-5, rtol=1e-5)
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_create_rotation_matrix_from_view_lookat_along_negative_up_axis(device):
+    """Camera below target looking up (-Z alignment with Z-up) should return a valid orthonormal frame."""
+    eyes = torch.tensor([[0.0, 0.0, -5.0]], device=device)
+    targets = torch.tensor([[0.0, 0.0, 0.0]], device=device)
+    R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device)
+    assert R is not None
+    identity = torch.eye(3, device=device).expand(1, 3, 3)
+    torch.testing.assert_close(R @ R.transpose(-1, -2), identity, atol=1e-5, rtol=1e-5)
+    torch.testing.assert_close(torch.linalg.det(R), torch.ones(1, device=device), atol=1e-5, rtol=1e-5)
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_create_rotation_matrix_from_view_zero_forward_returns_nan(device):
+    """When eyes == targets the forward direction is undefined; all entries of the row are NaN."""
+    eyes = torch.tensor([[1.0, 2.0, 3.0]], device=device)
+    targets = eyes.clone()
+    R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device)
+    assert torch.isnan(R).all()
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_create_rotation_matrix_from_view_batched_partial_failure(device):
+    """Mixed batch with one degenerate row should produce NaN in that row and a valid rotation in the other."""
+    eyes = torch.tensor([[1.0, 2.0, 3.0], [0.0, 0.0, 5.0]], device=device)
+    targets = torch.tensor([[1.0, 2.0, 3.0], [0.0, 0.0, 0.0]], device=device)
+    R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device)
+    assert torch.isnan(R[0]).any()
+    torch.testing.assert_close(R[1] @ R[1].T, torch.eye(3, device=device), atol=1e-5, rtol=1e-5)
+    torch.testing.assert_close(torch.linalg.det(R[1]), torch.tensor(1.0, device=device), atol=1e-5, rtol=1e-5)
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_quat_from_matrix_unit_norm_on_valid_input(device):
+    """quat_from_matrix should produce unit quaternions for any valid rotation matrix."""
+    n = 100
+    q_rand = math_utils.random_orientation(num=n, device=device)
+    rot_mat = math_utils.matrix_from_quat(q_rand)
+    q_value = math_utils.quat_from_matrix(rot_mat)
+    norms = torch.linalg.norm(q_value, dim=-1)
+    torch.testing.assert_close(norms, torch.ones(n, device=device), atol=1e-5, rtol=1e-5)
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_quat_from_matrix_singular_matrix_returns_nan(device):
+    """quat_from_matrix on a singular (non-rotation) matrix should signal NaN, not garbage."""
+    singular = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 1.0]]], device=device)
+    q = math_utils.quat_from_matrix(singular)
+    assert torch.isnan(q).all()
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_create_rotation_matrix_from_view_standard(device):
+    """Sanity: off-axis eye produces an orthonormal frame whose z-axis points from target back to eye."""
+    eyes = torch.tensor([[3.0, 0.0, 4.0]], device=device)
+    targets = torch.tensor([[0.0, 0.0, 0.0]], device=device)
+    R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device)
+    identity = torch.eye(3, device=device).expand(1, 3, 3)
+    torch.testing.assert_close(R @ R.transpose(-1, -2), identity, atol=1e-5, rtol=1e-5)
+    torch.testing.assert_close(torch.linalg.det(R), torch.ones(1, device=device), atol=1e-5, rtol=1e-5)
+    # z_axis is back-of-camera in OpenGL convention: points from target to eye
+    expected_z = torch.tensor([[0.6, 0.0, 0.8]], device=device)
+    torch.testing.assert_close(R[:, :, 2], expected_z, atol=1e-5, rtol=1e-5)
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_create_rotation_matrix_from_view_non_finite_returns_nan(device):
+    """Non-finite input (NaN or Inf in eyes/targets) should produce NaN rows."""
+    eyes = torch.tensor([[float("nan"), 0.0, 0.0]], device=device)
+    targets = torch.tensor([[0.0, 0.0, 0.0]], device=device)
+    R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device)
+    assert torch.isnan(R).all()
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_quat_from_matrix_reflection_returns_nan(device):
+    """A reflection matrix (det = -1) is not a proper rotation; the safeguard should signal NaN."""
+    reflection = torch.diag(torch.tensor([1.0, 1.0, -1.0], device=device)).unsqueeze(0)
+    q = math_utils.quat_from_matrix(reflection)
+    assert torch.isnan(q).all()
+
+
+@pytest.mark.parametrize("device", ("cpu", "cuda:0"))
+def test_quat_from_matrix_non_orthonormal_returns_nan(device):
+    """A non-orthonormal matrix (1% scale error on one axis) is not a valid rotation; expect NaN."""
+    R = torch.diag(torch.tensor([1.01, 1.0, 1.0], device=device)).unsqueeze(0)
+    q = math_utils.quat_from_matrix(R)
+    assert torch.isnan(q).all()

source/isaaclab_newton/changelog.d/jmart-singular-rotation.rst

@@ -0,0 +1,7 @@
+Fixed
+^^^^^
+
+* Fixed the acceleration-arrow debug visualizer in
+  :class:`~isaaclab_newton.sensors.pva.Pva` drawing arrows in undefined directions for
+  bodies with effectively zero acceleration. Such bodies are now skipped from the
+  visualization.