From c0c7906f3b96869c670aa6afebbc75d7a3ba8500 Mon Sep 17 00:00:00 2001 From: Jessica Martinez Date: Wed, 13 May 2026 17:04:14 -0500 Subject: [PATCH] OMPE-92490: Fix singular rotation matrix and non-rotation quaternion bugs - Make create_rotation_matrix_from_view emit a valid orthonormal frame when forward is parallel to up (via alt-up substitution), and fill NaN for rows with undefined forward (eyes == targets or non-finite input). - Add unit-norm safeguard to quat_from_matrix: non-rotation input (singular, reflection, scale-error) now returns NaN instead of silent garbage. - Update Camera and RayCasterCamera set_world_poses_from_view to skip degenerate rows with a warning and raise ValueError on full-batch failure. - Filter zero-acceleration bodies in Pva acceleration visualizers (Newton + PhysX) to avoid producing degenerate inputs. - Add 11 regression tests covering singular rotation, non-finite input, partial-batch failure, reflection matrices, and scale-error matrices. - Remove the 0.1 x-nudge workaround in test_ray_caster_camera and test_multi_mesh_ray_caster_camera now that the bare (0, 0, 5) input produces a valid rotation deterministically. --- .../changelog.d/jmart-singular-rotation.rst | 16 +++ .../isaaclab/sensors/camera/camera.py | 27 ++++- .../sensors/ray_caster/ray_caster_camera.py | 28 ++++- source/isaaclab/isaaclab/utils/math.py | 43 +++++-- .../test_multi_mesh_ray_caster_camera.py | 4 +- .../test/sensors/test_ray_caster_camera.py | 4 +- source/isaaclab/test/utils/test_math.py | 114 ++++++++++++++++++ .../changelog.d/jmart-singular-rotation.rst | 7 ++ .../isaaclab_newton/sensors/pva/pva.py | 29 +++-- .../changelog.d/jmart-singular-rotation.rst | 7 ++ .../isaaclab_physx/sensors/pva/pva.py | 29 +++-- 11 files changed, 266 insertions(+), 42 deletions(-) create mode 100644 source/isaaclab/changelog.d/jmart-singular-rotation.rst create mode 100644 source/isaaclab_newton/changelog.d/jmart-singular-rotation.rst create mode 100644 source/isaaclab_physx/changelog.d/jmart-singular-rotation.rst diff --git a/source/isaaclab/changelog.d/jmart-singular-rotation.rst b/source/isaaclab/changelog.d/jmart-singular-rotation.rst new file mode 100644 index 000000000000..61b703707531 --- /dev/null +++ b/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. diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index c481002e524e..1dc05becae9b 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/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) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py index 257b25698deb..a9b7239b8991 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py +++ b/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") """ diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index b01d7e7ab4fe..9b33252091aa 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/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: diff --git a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py index 7e7efe16d091..8657c938c691 100644 --- a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py +++ b/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), ) diff --git a/source/isaaclab/test/sensors/test_ray_caster_camera.py b/source/isaaclab/test/sensors/test_ray_caster_camera.py index 752734936934..cc10b092a806 100644 --- a/source/isaaclab/test/sensors/test_ray_caster_camera.py +++ b/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), ) diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index 6bff0b31e267..000bf00eb859 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/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() diff --git a/source/isaaclab_newton/changelog.d/jmart-singular-rotation.rst b/source/isaaclab_newton/changelog.d/jmart-singular-rotation.rst new file mode 100644 index 000000000000..3e479713b7b1 --- /dev/null +++ b/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. diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/pva/pva.py b/source/isaaclab_newton/isaaclab_newton/sensors/pva/pva.py index c000e17c437a..437140accdf6 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/pva/pva.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/pva/pva.py @@ -214,21 +214,28 @@ def _debug_vis_callback(self, event): # arrow scale default_scale = self.acceleration_visualizer.cfg.markers["arrow"].scale arrow_scale = torch.tensor(default_scale, device=self.device).repeat(self._data.lin_acc_b.torch.shape[0], 1) - # arrow direction from acceleration + # arrow direction from acceleration; filter out bodies with effectively zero accel (no defined direction) up_axis = UsdGeom.GetStageUpAxis(self.stage) pos_w_torch = self._data.pos_w.torch - quat_w_torch = self._data.quat_w.torch - lin_acc_b_torch = self._data.lin_acc_b.torch - quat_opengl = math_utils.quat_from_matrix( - math_utils.create_rotation_matrix_from_view( - pos_w_torch, - pos_w_torch + math_utils.quat_apply(quat_w_torch, lin_acc_b_torch), - up_axis=up_axis, - device=self._device, - ) + accel_w = math_utils.quat_apply(self._data.quat_w.torch, self._data.lin_acc_b.torch) + valid_indices = (torch.linalg.norm(accel_w, dim=-1) > 1e-5).nonzero(as_tuple=True)[0] + if valid_indices.numel() == 0: + return + pos_filtered = pos_w_torch.index_select(0, valid_indices) + accel_filtered = accel_w.index_select(0, valid_indices) + rotation_matrix = math_utils.create_rotation_matrix_from_view( + pos_filtered, + pos_filtered + accel_filtered, + up_axis=up_axis, + device=self._device, ) + quat_opengl = math_utils.quat_from_matrix(rotation_matrix) quat_w = math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world") - self.acceleration_visualizer.visualize(base_pos_w, quat_w, arrow_scale) + self.acceleration_visualizer.visualize( + base_pos_w.index_select(0, valid_indices), + quat_w, + arrow_scale.index_select(0, valid_indices), + ) def _invalidate_initialize_callback(self, event): """Clears references for re-initialization and re-registers with NewtonManager.""" diff --git a/source/isaaclab_physx/changelog.d/jmart-singular-rotation.rst b/source/isaaclab_physx/changelog.d/jmart-singular-rotation.rst new file mode 100644 index 000000000000..a2d6330e29b2 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/jmart-singular-rotation.rst @@ -0,0 +1,7 @@ +Fixed +^^^^^ + +* Fixed the acceleration-arrow debug visualizer in + :class:`~isaaclab_physx.sensors.pva.Pva` drawing arrows in undefined directions for + bodies with effectively zero acceleration. Such bodies are now skipped from the + visualization. diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py index 32f4549e416d..6f2ad9a70118 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py @@ -284,18 +284,25 @@ def _debug_vis_callback(self, event): arrow_scale = torch.tensor(default_scale, device=self.device).repeat(self._data.lin_acc_b.torch.shape[0], 1) # get up axis of current stage up_axis = UsdGeom.GetStageUpAxis(self.stage) - # arrow-direction + # arrow-direction; filter out bodies with effectively zero accel (no defined direction) pos_w_torch = self._data.pos_w.torch - quat_w_torch = self._data.quat_w.torch - lin_acc_b_torch = self._data.lin_acc_b.torch - quat_opengl = math_utils.quat_from_matrix( - math_utils.create_rotation_matrix_from_view( - pos_w_torch, - pos_w_torch + math_utils.quat_apply(quat_w_torch, lin_acc_b_torch), - up_axis=up_axis, - device=self._device, - ) + accel_w = math_utils.quat_apply(self._data.quat_w.torch, self._data.lin_acc_b.torch) + valid_indices = (torch.linalg.norm(accel_w, dim=-1) > 1e-5).nonzero(as_tuple=True)[0] + if valid_indices.numel() == 0: + return + pos_filtered = pos_w_torch.index_select(0, valid_indices) + accel_filtered = accel_w.index_select(0, valid_indices) + rotation_matrix = math_utils.create_rotation_matrix_from_view( + pos_filtered, + pos_filtered + accel_filtered, + up_axis=up_axis, + device=self._device, ) + quat_opengl = math_utils.quat_from_matrix(rotation_matrix) quat_w = math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world") # display markers - self.acceleration_visualizer.visualize(base_pos_w, quat_w, arrow_scale) + self.acceleration_visualizer.visualize( + base_pos_w.index_select(0, valid_indices), + quat_w, + arrow_scale.index_select(0, valid_indices), + )