From 2f3fc5895ec441079f890b459d29aecf85d5099b Mon Sep 17 00:00:00 2001
From: Rafael Wiltz <rwiltz@nvidia.com>
Date: Thu, 30 Jul 2026 16:27:46 -0400
Subject: [PATCH 1/5] Fix replay crash on trailing idle action in IK tasks

replay_demos.py stepped the environment one extra time after every
environment had exhausted its episodes: the step call sat outside the
has_next_action check, so the loop applied the untouched idle action
before terminating. No task defines idle_action, so it falls back to
zeros -- and for absolute task-space tasks a zeros action carries a
zero-norm quaternion.

DifferentialIKController.set_command renormalized that command as
quat / norm(quat), producing NaN. The NaN reached the joint position
targets, diverged the articulation, and surfaced on the next decimation
sub-step as an unrelated "torch.linalg.solve: the input matrix is
singular" error -- after all demonstrations had replayed successfully.

Stop the replay loop before the trailing step, and harden the
controller so a degenerate command holds the current end-effector
orientation instead of emitting NaN. The controller change is not
redundant: with --num_envs > 1 an environment that finishes early keeps
receiving the zero-quaternion idle action while the others replay, so
it would diverge that environment and take down the whole batch.

Also report a non-finite Jacobian in adaptive_dls by its actual cause
rather than as an opaque LAPACK singular-matrix failure, which is what
made this crash point away from the real defect.

Reported by QA replaying IsaacContrib-Stack-Cube-SO101-IK-Abs-v0.
---
 scripts/tools/replay_demos.py                 |  5 ++
 .../rwiltz-diff-ik-degenerate-quat.rst        | 11 ++++
 .../isaaclab/controllers/differential_ik.py   | 50 +++++++++++++++----
 .../test_differential_ik_features.py          | 48 ++++++++++++++++++
 4 files changed, 103 insertions(+), 11 deletions(-)
 create mode 100644 source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst

diff --git a/scripts/tools/replay_demos.py b/scripts/tools/replay_demos.py
index e8ecae95f118..c2d5771a164c 100644
--- a/scripts/tools/replay_demos.py
+++ b/scripts/tools/replay_demos.py
@@ -215,6 +215,11 @@ def replay_episodes_loop(  # noqa: C901
                     else:
                         has_next_action = True
                     actions[env_id] = env_next_action
+                if not has_next_action:
+                    # Every environment has exhausted its episodes. Stop before stepping so the
+                    # replay does not apply the idle action past the end of the recorded data --
+                    # for task-space (IK) tasks the zero idle action carries a zero-norm quaternion.
+                    break
                 if first_loop:
                     first_loop = False
                 else:
diff --git a/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst b/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst
new file mode 100644
index 000000000000..3fb2740de890
--- /dev/null
+++ b/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst
@@ -0,0 +1,11 @@
+Fixed
+^^^^^
+
+* Fixed :meth:`~isaaclab.controllers.DifferentialIKController.set_command` producing a NaN target
+  orientation when an absolute pose command carried a zero-norm quaternion. The NaN propagated into
+  the joint position targets and diverged the articulation, surfacing a step later as an unrelated
+  ``torch.linalg.solve ... input matrix is singular`` error. Degenerate quaternions now hold the
+  current end-effector orientation instead.
+* Fixed the ``adaptive_dls`` inverse-kinematics method reporting a non-finite Jacobian as an opaque
+  LAPACK singular-matrix or convergence failure. It now raises an error naming the actual cause: the
+  articulation state diverged before the solve.
diff --git a/source/isaaclab/isaaclab/controllers/differential_ik.py b/source/isaaclab/isaaclab/controllers/differential_ik.py
index 51f399ba4be7..6272cfd4cd30 100644
--- a/source/isaaclab/isaaclab/controllers/differential_ik.py
+++ b/source/isaaclab/isaaclab/controllers/differential_ik.py
@@ -14,6 +14,9 @@
 if TYPE_CHECKING:
     from .differential_ik_cfg import DifferentialIKControllerCfg
 
+_MIN_QUAT_NORM = 1e-6
+"""Smallest norm [dimensionless] a commanded quaternion may have before it is treated as degenerate."""
+
 
 class DifferentialIKController:
     r"""Differential inverse kinematics (IK) controller.
@@ -83,6 +86,8 @@ def __init__(self, cfg: DifferentialIKControllerCfg, num_envs: int, device: str)
         # -- optional joint position limits for null-space joint-limit avoidance (set externally)
         self._joint_pos_lower = None
         self._joint_pos_upper = None
+        # -- identity quaternion (x, y, z, w), the last-resort fallback for a degenerate command
+        self._identity_quat = torch.tensor([0.0, 0.0, 0.0, 1.0], device=self._device).repeat(self.num_envs, 1)
 
     """
     Properties.
@@ -158,9 +163,17 @@ def set_command(
                 self.ee_pos_des, self.ee_quat_des = apply_delta_pose(ee_pos, ee_quat, self._command)
             else:
                 self.ee_pos_des = self._command[:, 0:3]
-                # renormalize the commanded quaternion (callers may pass a slightly non-unit quat)
+                # renormalize the commanded quaternion (callers may pass a slightly non-unit quat).
+                # A zero-norm quaternion would divide by zero and yield NaN, which propagates
+                # silently into the joint position targets and only surfaces a step later as an
+                # unrelated solver failure. Hold the current end-effector orientation instead
+                # (identity when no current orientation was supplied) for those environments.
                 quat = self._command[:, 3:7]
-                self.ee_quat_des = quat / torch.linalg.norm(quat, dim=-1, keepdim=True)
+                quat_norm = torch.linalg.norm(quat, dim=-1, keepdim=True)
+                fallback_quat = self._identity_quat if ee_quat is None else ee_quat
+                self.ee_quat_des = torch.where(
+                    quat_norm > _MIN_QUAT_NORM, quat / quat_norm.clamp(min=_MIN_QUAT_NORM), fallback_quat
+                )
 
     def set_joint_pos_limits(self, lower: torch.Tensor, upper: torch.Tensor) -> None:
         """Provide the controlled joints' position limits for null-space joint-limit avoidance.
@@ -274,15 +287,30 @@ def _compute_delta_joint_pos(self, delta_pose: torch.Tensor, jacobian: torch.Ten
             # quadratically up to lambda_max^2 as the smallest task-Jacobian singular value -> 0
             # (Maciejewski-Klein). Keying off the full task Jacobian damps both position and
             # orientation rank-loss configurations.
-            sigma_min = torch.linalg.svdvals(jacobian)[:, -1]  # (N,)
-            ratio = (sigma_min / sigma_thresh).clamp(max=1.0)
-            lambda_sq = lambda_min**2 + (1.0 - ratio**2) * (lambda_max**2 - lambda_min**2)  # (N,)
-            jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2)
-            lambda_matrix = lambda_sq.view(-1, 1, 1) * torch.eye(n=jacobian.shape[1], device=self._device)
-            delta_joint_pos = torch.bmm(
-                jacobian_T,
-                torch.linalg.solve(torch.bmm(jacobian, jacobian_T) + lambda_matrix, delta_pose.unsqueeze(-1)),
-            ).squeeze(-1)
+            # Both decompositions below are well-posed for any finite Jacobian (the damped normal
+            # matrix is symmetric positive-definite), so a failure here means the Jacobian itself is
+            # non-finite -- i.e. the articulation state has already diverged upstream. Re-raise with
+            # that cause instead of the misleading "matrix is singular"/"failed to converge" that
+            # LAPACK reports. The check runs only on the failure path, so the happy path is unchanged.
+            try:
+                sigma_min = torch.linalg.svdvals(jacobian)[:, -1]  # (N,)
+                ratio = (sigma_min / sigma_thresh).clamp(max=1.0)
+                lambda_sq = lambda_min**2 + (1.0 - ratio**2) * (lambda_max**2 - lambda_min**2)  # (N,)
+                jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2)
+                lambda_matrix = lambda_sq.view(-1, 1, 1) * torch.eye(n=jacobian.shape[1], device=self._device)
+                damped_solution = torch.linalg.solve(
+                    torch.bmm(jacobian, jacobian_T) + lambda_matrix, delta_pose.unsqueeze(-1)
+                )
+            except torch.linalg.LinAlgError as err:
+                if not torch.isfinite(jacobian).all():
+                    raise RuntimeError(
+                        "Differential IK received a non-finite Jacobian, so the articulation state has already"
+                        " diverged (NaN/Inf) before this solve. This is usually caused by a NaN or degenerate"
+                        " (zero-norm quaternion) task-space command applied on an earlier step -- check the"
+                        " commands feeding the IK action term."
+                    ) from err
+                raise
+            delta_joint_pos = torch.bmm(jacobian_T, damped_solution).squeeze(-1)
         else:
             raise ValueError(f"Unsupported inverse-kinematics method: {self.cfg.ik_method}")
 
diff --git a/source/isaaclab/test/controllers/test_differential_ik_features.py b/source/isaaclab/test/controllers/test_differential_ik_features.py
index e4ac4a041efb..7f4dfca79cdf 100644
--- a/source/isaaclab/test/controllers/test_differential_ik_features.py
+++ b/source/isaaclab/test/controllers/test_differential_ik_features.py
@@ -86,6 +86,54 @@ def test_set_command_renormalizes_quat():
     torch.testing.assert_close(stored, raw / torch.linalg.norm(raw), atol=1e-6, rtol=0.0)
 
 
+def test_set_command_zero_quat_holds_current_orientation():
+    """A zero-norm commanded quaternion falls back to the current orientation instead of NaN.
+
+    Regression: dividing by a zero norm produced a NaN target orientation that propagated into the
+    joint position targets, diverged the articulation, and only surfaced a step later as an opaque
+    ``torch.linalg.solve ... matrix is singular`` failure.
+    """
+    c = _make_controller()
+    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
+    ee_quat = torch.tensor([_quat_xyzw([1.0, 0.0, 0.0], 0.5)])
+    cmd = torch.cat([ee_pos, torch.zeros(1, 4)], dim=-1)  # zero-norm quaternion
+    c.set_command(cmd, ee_pos, ee_quat)
+    assert torch.isfinite(c.ee_quat_des).all()
+    torch.testing.assert_close(c.ee_quat_des, ee_quat, atol=1e-6, rtol=0.0)
+
+
+def test_set_command_zero_quat_without_current_orientation_is_identity():
+    """Without a current orientation to hold, a zero-norm command falls back to identity."""
+    c = _make_controller()
+    cmd = torch.cat([torch.tensor([[0.3, -0.1, 0.2]]), torch.zeros(1, 4)], dim=-1)
+    c.set_command(cmd)
+    torch.testing.assert_close(c.ee_quat_des, torch.tensor([_ID_QUAT]), atol=1e-6, rtol=0.0)
+
+
+def test_set_command_zero_quat_only_affects_degenerate_envs():
+    """The fallback is per-environment: a valid command alongside a degenerate one is untouched."""
+    c = _make_controller(num_envs=2)
+    good_quat = _quat_xyzw([0.0, 1.0, 0.0], 0.4)
+    ee_pos = torch.tensor([[0.3, -0.1, 0.2], [0.3, -0.1, 0.2]])
+    ee_quat = torch.tensor([_ID_QUAT, _ID_QUAT])
+    cmd = torch.tensor([[0.3, -0.1, 0.2] + good_quat, [0.3, -0.1, 0.2, 0.0, 0.0, 0.0, 0.0]])
+    c.set_command(cmd, ee_pos, ee_quat)
+    torch.testing.assert_close(c.ee_quat_des[0], torch.tensor(good_quat), atol=1e-6, rtol=0.0)
+    torch.testing.assert_close(c.ee_quat_des[1], torch.tensor(_ID_QUAT), atol=1e-6, rtol=0.0)
+
+
+def test_adaptive_dls_reports_non_finite_jacobian_cause():
+    """A non-finite Jacobian raises an error naming the real cause, not "matrix is singular"."""
+    c = _make_controller(ik_method="adaptive_dls")
+    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
+    ee_quat = torch.tensor([_ID_QUAT])
+    c.set_command(torch.tensor([[0.31, -0.1, 0.2] + _ID_QUAT]), ee_pos, ee_quat)
+    jac = torch.zeros(1, 6, _NUM_JOINTS)
+    jac[0, 0, 0] = float("nan")
+    with pytest.raises(RuntimeError, match="non-finite Jacobian"):
+        c.compute(ee_pos, ee_quat, jac, torch.zeros(1, _NUM_JOINTS))
+
+
 def test_orientation_weight_none_is_unweighted():
     """With no orientation weight, the pose task Jacobian equals the raw Jacobian."""
     c = _make_controller(orientation_weight=None)

From 3915af09ba1b9a2dfb1cdbd2dba29de0c3731b73 Mon Sep 17 00:00:00 2001
From: Rafael Wiltz <rwiltz@nvidia.com>
Date: Thu, 30 Jul 2026 16:47:09 -0400
Subject: [PATCH 2/5] Check Jacobian finiteness before the adaptive DLS solve

The non-finite Jacobian diagnostic was reached only when svdvals or
solve raised LinAlgError, so it depended on backend error-reporting
behavior. Backends that propagate NaN instead of raising bypassed it
and let non-finite joint position targets reach the articulation. An
Inf-valued Jacobian took that path even on CPU.

Check finiteness up front instead, which covers both behaviors and
drops the try/except. The check costs one device sync (~32us, flat in
batch size); adaptive_dls is used only by the SO-101 teleop and replay
task at frame rate, where that is immaterial.

Extend the regression test over NaN, +Inf and -Inf, and add a case
asserting a well-conditioned Jacobian is unaffected by the guard.
---
 .../rwiltz-diff-ik-degenerate-quat.rst        |  5 ++-
 .../isaaclab/controllers/differential_ik.py   | 42 +++++++++----------
 .../test_differential_ik_features.py          | 26 ++++++++++--
 3 files changed, 45 insertions(+), 28 deletions(-)

diff --git a/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst b/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst
index 3fb2740de890..c500f28fa2ed 100644
--- a/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst
+++ b/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst
@@ -6,6 +6,7 @@ Fixed
   the joint position targets and diverged the articulation, surfacing a step later as an unrelated
   ``torch.linalg.solve ... input matrix is singular`` error. Degenerate quaternions now hold the
   current end-effector orientation instead.
-* Fixed the ``adaptive_dls`` inverse-kinematics method reporting a non-finite Jacobian as an opaque
-  LAPACK singular-matrix or convergence failure. It now raises an error naming the actual cause: the
+* Fixed the ``adaptive_dls`` inverse-kinematics method handling of a non-finite Jacobian, which
+  backends either reported as an opaque LAPACK singular-matrix or convergence failure, or propagated
+  silently into the joint position targets. It now raises an error naming the actual cause: the
   articulation state diverged before the solve.
diff --git a/source/isaaclab/isaaclab/controllers/differential_ik.py b/source/isaaclab/isaaclab/controllers/differential_ik.py
index 6272cfd4cd30..3cebbd677de8 100644
--- a/source/isaaclab/isaaclab/controllers/differential_ik.py
+++ b/source/isaaclab/isaaclab/controllers/differential_ik.py
@@ -288,29 +288,27 @@ def _compute_delta_joint_pos(self, delta_pose: torch.Tensor, jacobian: torch.Ten
             # (Maciejewski-Klein). Keying off the full task Jacobian damps both position and
             # orientation rank-loss configurations.
             # Both decompositions below are well-posed for any finite Jacobian (the damped normal
-            # matrix is symmetric positive-definite), so a failure here means the Jacobian itself is
-            # non-finite -- i.e. the articulation state has already diverged upstream. Re-raise with
-            # that cause instead of the misleading "matrix is singular"/"failed to converge" that
-            # LAPACK reports. The check runs only on the failure path, so the happy path is unchanged.
-            try:
-                sigma_min = torch.linalg.svdvals(jacobian)[:, -1]  # (N,)
-                ratio = (sigma_min / sigma_thresh).clamp(max=1.0)
-                lambda_sq = lambda_min**2 + (1.0 - ratio**2) * (lambda_max**2 - lambda_min**2)  # (N,)
-                jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2)
-                lambda_matrix = lambda_sq.view(-1, 1, 1) * torch.eye(n=jacobian.shape[1], device=self._device)
-                damped_solution = torch.linalg.solve(
-                    torch.bmm(jacobian, jacobian_T) + lambda_matrix, delta_pose.unsqueeze(-1)
+            # matrix is symmetric positive-definite), so a non-finite Jacobian is the only way they
+            # can fail -- it means the articulation state has already diverged upstream. Check it up
+            # front rather than reacting to a decomposition error: depending on the backend, LAPACK
+            # either reports a misleading "matrix is singular"/"failed to converge" or propagates the
+            # NaN silently into the joint targets. Failing here names the real cause in both cases.
+            if not torch.isfinite(jacobian).all():
+                raise RuntimeError(
+                    "Differential IK received a non-finite Jacobian, so the articulation state has already"
+                    " diverged (NaN/Inf) before this solve. This is usually caused by a NaN or degenerate"
+                    " (zero-norm quaternion) task-space command applied on an earlier step -- check the"
+                    " commands feeding the IK action term."
                 )
-            except torch.linalg.LinAlgError as err:
-                if not torch.isfinite(jacobian).all():
-                    raise RuntimeError(
-                        "Differential IK received a non-finite Jacobian, so the articulation state has already"
-                        " diverged (NaN/Inf) before this solve. This is usually caused by a NaN or degenerate"
-                        " (zero-norm quaternion) task-space command applied on an earlier step -- check the"
-                        " commands feeding the IK action term."
-                    ) from err
-                raise
-            delta_joint_pos = torch.bmm(jacobian_T, damped_solution).squeeze(-1)
+            sigma_min = torch.linalg.svdvals(jacobian)[:, -1]  # (N,)
+            ratio = (sigma_min / sigma_thresh).clamp(max=1.0)
+            lambda_sq = lambda_min**2 + (1.0 - ratio**2) * (lambda_max**2 - lambda_min**2)  # (N,)
+            jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2)
+            lambda_matrix = lambda_sq.view(-1, 1, 1) * torch.eye(n=jacobian.shape[1], device=self._device)
+            delta_joint_pos = torch.bmm(
+                jacobian_T,
+                torch.linalg.solve(torch.bmm(jacobian, jacobian_T) + lambda_matrix, delta_pose.unsqueeze(-1)),
+            ).squeeze(-1)
         else:
             raise ValueError(f"Unsupported inverse-kinematics method: {self.cfg.ik_method}")
 
diff --git a/source/isaaclab/test/controllers/test_differential_ik_features.py b/source/isaaclab/test/controllers/test_differential_ik_features.py
index 7f4dfca79cdf..daa036739ec0 100644
--- a/source/isaaclab/test/controllers/test_differential_ik_features.py
+++ b/source/isaaclab/test/controllers/test_differential_ik_features.py
@@ -122,18 +122,36 @@ def test_set_command_zero_quat_only_affects_degenerate_envs():
     torch.testing.assert_close(c.ee_quat_des[1], torch.tensor(_ID_QUAT), atol=1e-6, rtol=0.0)
 
 
-def test_adaptive_dls_reports_non_finite_jacobian_cause():
-    """A non-finite Jacobian raises an error naming the real cause, not "matrix is singular"."""
+@pytest.mark.parametrize("bad_value", [float("nan"), float("inf"), float("-inf")])
+def test_adaptive_dls_reports_non_finite_jacobian_cause(bad_value):
+    """A non-finite Jacobian raises an error naming the real cause, not "matrix is singular".
+
+    The check is unconditional rather than a reaction to a decomposition error: backends differ in
+    whether ``svdvals``/``solve`` raise on non-finite input or propagate NaN silently, and a silent
+    NaN would otherwise reach the joint position targets.
+    """
     c = _make_controller(ik_method="adaptive_dls")
     ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
     ee_quat = torch.tensor([_ID_QUAT])
     c.set_command(torch.tensor([[0.31, -0.1, 0.2] + _ID_QUAT]), ee_pos, ee_quat)
-    jac = torch.zeros(1, 6, _NUM_JOINTS)
-    jac[0, 0, 0] = float("nan")
+    # otherwise well-conditioned, so only the non-finite entry can trigger the failure
+    jac = torch.eye(6, _NUM_JOINTS).unsqueeze(0).clone()
+    jac[0, 0, 0] = bad_value
     with pytest.raises(RuntimeError, match="non-finite Jacobian"):
         c.compute(ee_pos, ee_quat, jac, torch.zeros(1, _NUM_JOINTS))
 
 
+def test_adaptive_dls_finite_jacobian_is_unaffected_by_the_guard():
+    """The non-finite guard does not change results for a well-conditioned Jacobian."""
+    c = _make_controller(ik_method="adaptive_dls")
+    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
+    ee_quat = torch.tensor([_ID_QUAT])
+    c.set_command(torch.tensor([[0.31, -0.1, 0.2] + _ID_QUAT]), ee_pos, ee_quat)
+    jac = torch.eye(6, _NUM_JOINTS).unsqueeze(0).clone()
+    out = c.compute(ee_pos, ee_quat, jac, torch.zeros(1, _NUM_JOINTS))
+    assert torch.isfinite(out).all()
+
+
 def test_orientation_weight_none_is_unweighted():
     """With no orientation weight, the pose task Jacobian equals the raw Jacobian."""
     c = _make_controller(orientation_weight=None)

From 8decaeed806c71419bcb5e0c3f43a5cbddc466ba Mon Sep 17 00:00:00 2001
From: Rafael Wiltz <rwiltz@nvidia.com>
Date: Thu, 30 Jul 2026 16:51:47 -0400
Subject: [PATCH 3/5] Narrow the degenerate quaternion test to normalization
 failure

The 1e-6 norm cutoff was broader than the crash it fixes. A quaternion
such as [0, 0, 0, 1e-7] normalizes cleanly to identity and did so
before this branch, but the cutoff silently replaced it with the
fallback orientation -- a behavior change beyond the zero-norm fix.

Decide degeneracy by whether the normalization produced a finite
result instead. This drops the magic constant and is exact at both
ends: a zero quaternion (0/0) and a norm that underflows to zero
(Inf) are still caught, while anything that normalizes keeps its
previous meaning. A plain norm > 0 test would not cover underflow.

Document the fallback in set_command, including ee_quat's role for
absolute pose commands, which the docstring previously described as
relevant only to position and relative-pose modes.
---
 .../isaaclab/controllers/differential_ik.py   | 24 +++++++++++------
 .../test_differential_ik_features.py          | 26 +++++++++++++++++++
 2 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/source/isaaclab/isaaclab/controllers/differential_ik.py b/source/isaaclab/isaaclab/controllers/differential_ik.py
index 3cebbd677de8..3ad46d3f5725 100644
--- a/source/isaaclab/isaaclab/controllers/differential_ik.py
+++ b/source/isaaclab/isaaclab/controllers/differential_ik.py
@@ -14,9 +14,6 @@
 if TYPE_CHECKING:
     from .differential_ik_cfg import DifferentialIKControllerCfg
 
-_MIN_QUAT_NORM = 1e-6
-"""Smallest norm [dimensionless] a commanded quaternion may have before it is treated as degenerate."""
-
 
 class DifferentialIKController:
     r"""Differential inverse kinematics (IK) controller.
@@ -124,12 +121,20 @@ def set_command(
         It is up to the user to ensure that the command is given in the correct frame. The method only
         applies the relative mode if the command type is ``position_rel`` or ``pose_rel``.
 
+        For absolute ``pose`` commands the commanded quaternion is renormalized, so a slightly
+        non-unit quaternion is accepted. A *degenerate* quaternion -- one that cannot be normalized
+        to a finite value, i.e. a zero quaternion or one whose norm underflows to zero -- would
+        otherwise yield NaN, so those entries fall back per-environment to :attr:`ee_quat` (holding
+        the current orientation), or to identity when :attr:`ee_quat` is not provided.
+
         Args:
             command: The input command in shape (N, 3) or (N, 6) or (N, 7).
             ee_pos: The current end-effector position in shape (N, 3).
                 This is only needed if the command type is ``position_rel`` or ``pose_rel``.
             ee_quat: The current end-effector orientation (x, y, z, w) in shape (N, 4).
-                This is only needed if the command type is ``position_*`` or ``pose_rel``.
+                This is needed if the command type is ``position_*`` or ``pose_rel``. For absolute
+                ``pose`` commands it is optional and used only as the fallback orientation for a
+                degenerate commanded quaternion (see above).
 
         Raises:
             ValueError: If the command type is ``position_*`` and :attr:`ee_quat` is None.
@@ -169,11 +174,14 @@ def set_command(
                 # unrelated solver failure. Hold the current end-effector orientation instead
                 # (identity when no current orientation was supplied) for those environments.
                 quat = self._command[:, 3:7]
-                quat_norm = torch.linalg.norm(quat, dim=-1, keepdim=True)
+                normalized_quat = quat / torch.linalg.norm(quat, dim=-1, keepdim=True)
+                # Degeneracy is decided by whether the normalization produced a finite result, not by
+                # a magnitude threshold: a zero-norm quaternion gives 0/0, and a norm that underflows
+                # to zero gives Inf. Any quaternion that normalizes cleanly keeps its previous
+                # meaning, however small its norm (e.g. ``[0, 0, 0, 1e-7]`` is still identity).
+                is_valid = torch.isfinite(normalized_quat).all(dim=-1, keepdim=True)
                 fallback_quat = self._identity_quat if ee_quat is None else ee_quat
-                self.ee_quat_des = torch.where(
-                    quat_norm > _MIN_QUAT_NORM, quat / quat_norm.clamp(min=_MIN_QUAT_NORM), fallback_quat
-                )
+                self.ee_quat_des = torch.where(is_valid, normalized_quat, fallback_quat)
 
     def set_joint_pos_limits(self, lower: torch.Tensor, upper: torch.Tensor) -> None:
         """Provide the controlled joints' position limits for null-space joint-limit avoidance.
diff --git a/source/isaaclab/test/controllers/test_differential_ik_features.py b/source/isaaclab/test/controllers/test_differential_ik_features.py
index daa036739ec0..64cb0c7c2308 100644
--- a/source/isaaclab/test/controllers/test_differential_ik_features.py
+++ b/source/isaaclab/test/controllers/test_differential_ik_features.py
@@ -102,6 +102,32 @@ def test_set_command_zero_quat_holds_current_orientation():
     torch.testing.assert_close(c.ee_quat_des, ee_quat, atol=1e-6, rtol=0.0)
 
 
+@pytest.mark.parametrize("scale", [1e-7, 1e-20])
+def test_set_command_tiny_nonzero_quat_is_still_normalized(scale):
+    """A tiny but normalizable quaternion keeps its meaning instead of taking the fallback.
+
+    Degeneracy is decided by whether the normalization yields a finite result, not by a magnitude
+    threshold, so commands that normalized cleanly before are unaffected.
+    """
+    c = _make_controller()
+    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
+    ee_quat = torch.tensor([_quat_xyzw([1.0, 0.0, 0.0], 0.5)])  # a fallback that is NOT identity
+    cmd = torch.cat([ee_pos, torch.tensor([[0.0, 0.0, 0.0, scale]])], dim=-1)  # scaled identity
+    c.set_command(cmd, ee_pos, ee_quat)
+    torch.testing.assert_close(c.ee_quat_des, torch.tensor([_ID_QUAT]), atol=1e-5, rtol=0.0)
+
+
+def test_set_command_underflowing_quat_norm_takes_fallback():
+    """A quaternion whose norm underflows to zero cannot be normalized, so it takes the fallback."""
+    c = _make_controller()
+    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
+    ee_quat = torch.tensor([_quat_xyzw([1.0, 0.0, 0.0], 0.5)])
+    cmd = torch.cat([ee_pos, torch.tensor([[0.0, 0.0, 0.0, 1e-38]])], dim=-1)
+    c.set_command(cmd, ee_pos, ee_quat)
+    assert torch.isfinite(c.ee_quat_des).all()
+    torch.testing.assert_close(c.ee_quat_des, ee_quat, atol=1e-6, rtol=0.0)
+
+
 def test_set_command_zero_quat_without_current_orientation_is_identity():
     """Without a current orientation to hold, a zero-norm command falls back to identity."""
     c = _make_controller()

From 8398a5a6fc459776006ece7b83aa756282dbc7fd Mon Sep 17 00:00:00 2001
From: Rafael Wiltz <rwiltz@nvidia.com>
Date: Mon, 3 Aug 2026 16:54:29 -0400
Subject: [PATCH 4/5] Add regression test for replay loop termination

The replay-loop fix had no direct coverage: the controller tests still
passed with the break removed, so the extra idle step could return
unnoticed. Exercise replay_episodes_loop against a stub environment and
dataset, asserting one env.step per recorded action and no trailing idle
step. The script launches the simulator at import time, so the loop is
extracted from the source and executed in isolation.

Also trim the loop comment to the local, functional statement; the IK
failure chain is incident history recorded in the PR.
---
 scripts/tools/replay_demos.py                 |   4 +-
 .../cli/test_replay_demos_loop_termination.py | 111 ++++++++++++++++++
 2 files changed, 112 insertions(+), 3 deletions(-)
 create mode 100644 source/isaaclab/test/cli/test_replay_demos_loop_termination.py

diff --git a/scripts/tools/replay_demos.py b/scripts/tools/replay_demos.py
index c2d5771a164c..dd3273b9f1c8 100644
--- a/scripts/tools/replay_demos.py
+++ b/scripts/tools/replay_demos.py
@@ -216,9 +216,7 @@ def replay_episodes_loop(  # noqa: C901
                         has_next_action = True
                     actions[env_id] = env_next_action
                 if not has_next_action:
-                    # Every environment has exhausted its episodes. Stop before stepping so the
-                    # replay does not apply the idle action past the end of the recorded data --
-                    # for task-space (IK) tasks the zero idle action carries a zero-norm quaternion.
+                    # Stop before stepping once every environment has exhausted its recorded actions.
                     break
                 if first_loop:
                     first_loop = False
diff --git a/source/isaaclab/test/cli/test_replay_demos_loop_termination.py b/source/isaaclab/test/cli/test_replay_demos_loop_termination.py
new file mode 100644
index 000000000000..d390abc14fc1
--- /dev/null
+++ b/source/isaaclab/test/cli/test_replay_demos_loop_termination.py
@@ -0,0 +1,111 @@
+# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Regression test for the replay loop stepping past the end of the recorded data.
+
+``replay_episodes_loop`` used to call ``env.step`` once more after every environment had exhausted
+its episodes, applying the untouched idle action. For a task-space (IK) task that idle action is
+all zeros, i.e. a zero-norm quaternion, which crashed the run after a successful replay.
+
+``scripts/tools/replay_demos.py`` launches the simulator at import time, so the loop function is
+extracted from the source and executed against stub objects instead.
+"""
+
+import ast
+import contextlib
+from pathlib import Path
+
+import pytest
+import torch
+
+from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler
+
+pytestmark = pytest.mark.integration
+
+# This test lives at source/isaaclab/test/cli/test_replay_demos_loop_termination.py.
+_REPLAY_DEMOS_PATH = Path(__file__).resolve().parents[4] / "scripts" / "tools" / "replay_demos.py"
+
+
+def _load_replay_episodes_loop(simulation_app):
+    """Compile ``replay_episodes_loop`` from the script source, bound to the given app stub."""
+    source = _REPLAY_DEMOS_PATH.read_text()
+    tree = ast.parse(source)
+    func = next(node for node in tree.body if isinstance(node, ast.FunctionDef) and node.name == "replay_episodes_loop")
+    namespace = {
+        "contextlib": contextlib,
+        "torch": torch,
+        "EpisodeData": EpisodeData,
+        "HDF5DatasetFileHandler": HDF5DatasetFileHandler,
+        "simulation_app": simulation_app,
+        "is_paused": False,
+    }
+    exec(compile(ast.Module(body=[func], type_ignores=[]), str(_REPLAY_DEMOS_PATH), "exec"), namespace)
+    return namespace["replay_episodes_loop"]
+
+
+class _SimulationAppStub:
+    def is_running(self):
+        return True
+
+    def is_exiting(self):
+        return False
+
+
+class _EnvStub:
+    """Records the actions passed to :meth:`step`."""
+
+    device = "cpu"
+
+    def __init__(self):
+        self.stepped_actions: list[torch.Tensor] = []
+
+    def reset_to(self, state, env_ids, is_relative=True):
+        pass
+
+    def step(self, actions):
+        self.stepped_actions.append(actions.clone())
+
+
+class _DatasetFileHandlerStub:
+    def __init__(self, actions: torch.Tensor):
+        self._actions = actions
+
+    def load_episode(self, episode_name, device):
+        episode = EpisodeData()
+        episode.data = {"initial_state": {}, "actions": list(self._actions)}
+        return episode
+
+
+def test_replay_loop_does_not_step_after_the_recorded_actions():
+    """The loop steps exactly once per recorded action and never applies the idle action."""
+    # absolute task-space actions: [pos_xyz, quat_xyzw, gripper]
+    recorded_actions = torch.tensor(
+        [
+            [0.30, -0.10, 0.20, 0.0, 0.0, 0.0, 1.0, 0.0],
+            [0.31, -0.10, 0.20, 0.0, 0.0, 0.0, 1.0, 0.0],
+            [0.32, -0.10, 0.20, 0.0, 0.0, 0.0, 1.0, 0.0],
+        ]
+    )
+    idle_action = torch.zeros(1, recorded_actions.shape[-1])
+    env = _EnvStub()
+    replay_episodes_loop = _load_replay_episodes_loop(_SimulationAppStub())
+
+    replayed_episode_count, _, _ = replay_episodes_loop(
+        env,
+        _DatasetFileHandlerStub(recorded_actions),
+        episode_names=["demo_0"],
+        episode_count=1,
+        episode_indices_to_replay=[0],
+        num_envs=1,
+        success_term=None,
+        state_validation_enabled=False,
+        idle_action=idle_action,
+        reset_sim_buffer_each_episode=False,
+    )
+
+    assert replayed_episode_count == 1
+    assert len(env.stepped_actions) == len(recorded_actions)
+    for stepped, recorded in zip(env.stepped_actions, recorded_actions):
+        torch.testing.assert_close(stepped, recorded.unsqueeze(0), atol=1e-6, rtol=0.0)

From 7798fef2f1c156bce387f32d40f879a51a975439 Mon Sep 17 00:00:00 2001
From: Rafael Wiltz <rwiltz@nvidia.com>
Date: Mon, 3 Aug 2026 16:54:29 -0400
Subject: [PATCH 5/5] Drop adaptive DLS Jacobian guard and tighten IK docs

The unconditional torch.isfinite(jacobian).all() check converted a device
reduction to a Python bool on every adaptive-DLS control step, forcing a
host sync solely to improve an exceptional error message. A non-finite
Jacobian also does not prove the articulation diverged, and finite inputs
do not guarantee finite downstream arithmetic, so the diagnostic claimed
more than it could establish. Remove it along with its tests.

State the quaternion fallback contract directly in the public docstring
using :paramref:, replace the narrated comments with one functional line,
consolidate the overlapping quaternion regressions, and make the
changelog outcome-focused so it also covers the replay-loop fix.
---
 .../rwiltz-diff-ik-degenerate-quat.rst        | 13 +--
 .../isaaclab/controllers/differential_ik.py   | 34 +------
 .../test_differential_ik_features.py          | 99 ++++---------------
 3 files changed, 29 insertions(+), 117 deletions(-)

diff --git a/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst b/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst
index c500f28fa2ed..ebf049384a8d 100644
--- a/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst
+++ b/source/isaaclab/changelog.d/rwiltz-diff-ik-degenerate-quat.rst
@@ -1,12 +1,7 @@
 Fixed
 ^^^^^
 
-* Fixed :meth:`~isaaclab.controllers.DifferentialIKController.set_command` producing a NaN target
-  orientation when an absolute pose command carried a zero-norm quaternion. The NaN propagated into
-  the joint position targets and diverged the articulation, surfacing a step later as an unrelated
-  ``torch.linalg.solve ... input matrix is singular`` error. Degenerate quaternions now hold the
-  current end-effector orientation instead.
-* Fixed the ``adaptive_dls`` inverse-kinematics method handling of a non-finite Jacobian, which
-  backends either reported as an opaque LAPACK singular-matrix or convergence failure, or propagated
-  silently into the joint position targets. It now raises an error naming the actual cause: the
-  articulation state diverged before the solve.
+* Fixed demonstration replay stepping once after all episodes completed.
+* Fixed :meth:`~isaaclab.controllers.DifferentialIKController.set_command` handling of
+  unnormalizable absolute-pose quaternions, which produced a NaN target orientation. Such
+  commands now hold the current end-effector orientation, or identity when none is provided.
diff --git a/source/isaaclab/isaaclab/controllers/differential_ik.py b/source/isaaclab/isaaclab/controllers/differential_ik.py
index 3ad46d3f5725..08b469a41ed3 100644
--- a/source/isaaclab/isaaclab/controllers/differential_ik.py
+++ b/source/isaaclab/isaaclab/controllers/differential_ik.py
@@ -121,11 +121,8 @@ def set_command(
         It is up to the user to ensure that the command is given in the correct frame. The method only
         applies the relative mode if the command type is ``position_rel`` or ``pose_rel``.
 
-        For absolute ``pose`` commands the commanded quaternion is renormalized, so a slightly
-        non-unit quaternion is accepted. A *degenerate* quaternion -- one that cannot be normalized
-        to a finite value, i.e. a zero quaternion or one whose norm underflows to zero -- would
-        otherwise yield NaN, so those entries fall back per-environment to :attr:`ee_quat` (holding
-        the current orientation), or to identity when :attr:`ee_quat` is not provided.
+        Absolute ``pose`` commands normalize finite quaternions; unnormalizable entries use
+        :paramref:`ee_quat`, or identity when :paramref:`ee_quat` is omitted.
 
         Args:
             command: The input command in shape (N, 3) or (N, 6) or (N, 7).
@@ -133,8 +130,8 @@ def set_command(
                 This is only needed if the command type is ``position_rel`` or ``pose_rel``.
             ee_quat: The current end-effector orientation (x, y, z, w) in shape (N, 4).
                 This is needed if the command type is ``position_*`` or ``pose_rel``. For absolute
-                ``pose`` commands it is optional and used only as the fallback orientation for a
-                degenerate commanded quaternion (see above).
+                ``pose`` commands it is optional and used only as the fallback orientation for an
+                unnormalizable commanded quaternion.
 
         Raises:
             ValueError: If the command type is ``position_*`` and :attr:`ee_quat` is None.
@@ -168,17 +165,9 @@ def set_command(
                 self.ee_pos_des, self.ee_quat_des = apply_delta_pose(ee_pos, ee_quat, self._command)
             else:
                 self.ee_pos_des = self._command[:, 0:3]
-                # renormalize the commanded quaternion (callers may pass a slightly non-unit quat).
-                # A zero-norm quaternion would divide by zero and yield NaN, which propagates
-                # silently into the joint position targets and only surfaces a step later as an
-                # unrelated solver failure. Hold the current end-effector orientation instead
-                # (identity when no current orientation was supplied) for those environments.
+                # normalize valid quaternions and use the fallback for non-finite results
                 quat = self._command[:, 3:7]
                 normalized_quat = quat / torch.linalg.norm(quat, dim=-1, keepdim=True)
-                # Degeneracy is decided by whether the normalization produced a finite result, not by
-                # a magnitude threshold: a zero-norm quaternion gives 0/0, and a norm that underflows
-                # to zero gives Inf. Any quaternion that normalizes cleanly keeps its previous
-                # meaning, however small its norm (e.g. ``[0, 0, 0, 1e-7]`` is still identity).
                 is_valid = torch.isfinite(normalized_quat).all(dim=-1, keepdim=True)
                 fallback_quat = self._identity_quat if ee_quat is None else ee_quat
                 self.ee_quat_des = torch.where(is_valid, normalized_quat, fallback_quat)
@@ -295,19 +284,6 @@ def _compute_delta_joint_pos(self, delta_pose: torch.Tensor, jacobian: torch.Ten
             # quadratically up to lambda_max^2 as the smallest task-Jacobian singular value -> 0
             # (Maciejewski-Klein). Keying off the full task Jacobian damps both position and
             # orientation rank-loss configurations.
-            # Both decompositions below are well-posed for any finite Jacobian (the damped normal
-            # matrix is symmetric positive-definite), so a non-finite Jacobian is the only way they
-            # can fail -- it means the articulation state has already diverged upstream. Check it up
-            # front rather than reacting to a decomposition error: depending on the backend, LAPACK
-            # either reports a misleading "matrix is singular"/"failed to converge" or propagates the
-            # NaN silently into the joint targets. Failing here names the real cause in both cases.
-            if not torch.isfinite(jacobian).all():
-                raise RuntimeError(
-                    "Differential IK received a non-finite Jacobian, so the articulation state has already"
-                    " diverged (NaN/Inf) before this solve. This is usually caused by a NaN or degenerate"
-                    " (zero-norm quaternion) task-space command applied on an earlier step -- check the"
-                    " commands feeding the IK action term."
-                )
             sigma_min = torch.linalg.svdvals(jacobian)[:, -1]  # (N,)
             ratio = (sigma_min / sigma_thresh).clamp(max=1.0)
             lambda_sq = lambda_min**2 + (1.0 - ratio**2) * (lambda_max**2 - lambda_min**2)  # (N,)
diff --git a/source/isaaclab/test/controllers/test_differential_ik_features.py b/source/isaaclab/test/controllers/test_differential_ik_features.py
index 64cb0c7c2308..1789fc71da3c 100644
--- a/source/isaaclab/test/controllers/test_differential_ik_features.py
+++ b/source/isaaclab/test/controllers/test_differential_ik_features.py
@@ -86,29 +86,31 @@ def test_set_command_renormalizes_quat():
     torch.testing.assert_close(stored, raw / torch.linalg.norm(raw), atol=1e-6, rtol=0.0)
 
 
-def test_set_command_zero_quat_holds_current_orientation():
-    """A zero-norm commanded quaternion falls back to the current orientation instead of NaN.
+@pytest.mark.parametrize("bad_quat", [[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1e-38]])
+def test_set_command_unnormalizable_quat_holds_current_orientation(bad_quat):
+    """An unnormalizable commanded quaternion holds that env's current orientation instead of NaN."""
+    c = _make_controller(num_envs=2)
+    good_quat = _quat_xyzw([0.0, 1.0, 0.0], 0.4)
+    held_quat = _quat_xyzw([1.0, 0.0, 0.0], 0.5)
+    ee_pos = torch.tensor([[0.3, -0.1, 0.2], [0.3, -0.1, 0.2]])
+    ee_quat = torch.tensor([_ID_QUAT, held_quat])
+    cmd = torch.tensor([[0.3, -0.1, 0.2] + good_quat, [0.3, -0.1, 0.2] + bad_quat])
+    c.set_command(cmd, ee_pos, ee_quat)
+    torch.testing.assert_close(c.ee_quat_des[0], torch.tensor(good_quat), atol=1e-6, rtol=0.0)
+    torch.testing.assert_close(c.ee_quat_des[1], torch.tensor(held_quat), atol=1e-6, rtol=0.0)
 
-    Regression: dividing by a zero norm produced a NaN target orientation that propagated into the
-    joint position targets, diverged the articulation, and only surfaced a step later as an opaque
-    ``torch.linalg.solve ... matrix is singular`` failure.
-    """
+
+def test_set_command_unnormalizable_quat_without_current_orientation_is_identity():
+    """Without a current orientation to hold, an unnormalizable command falls back to identity."""
     c = _make_controller()
-    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
-    ee_quat = torch.tensor([_quat_xyzw([1.0, 0.0, 0.0], 0.5)])
-    cmd = torch.cat([ee_pos, torch.zeros(1, 4)], dim=-1)  # zero-norm quaternion
-    c.set_command(cmd, ee_pos, ee_quat)
-    assert torch.isfinite(c.ee_quat_des).all()
-    torch.testing.assert_close(c.ee_quat_des, ee_quat, atol=1e-6, rtol=0.0)
+    cmd = torch.cat([torch.tensor([[0.3, -0.1, 0.2]]), torch.zeros(1, 4)], dim=-1)
+    c.set_command(cmd)
+    torch.testing.assert_close(c.ee_quat_des, torch.tensor([_ID_QUAT]), atol=1e-6, rtol=0.0)
 
 
 @pytest.mark.parametrize("scale", [1e-7, 1e-20])
-def test_set_command_tiny_nonzero_quat_is_still_normalized(scale):
-    """A tiny but normalizable quaternion keeps its meaning instead of taking the fallback.
-
-    Degeneracy is decided by whether the normalization yields a finite result, not by a magnitude
-    threshold, so commands that normalized cleanly before are unaffected.
-    """
+def test_set_command_tiny_normalizable_quat_is_still_normalized(scale):
+    """A tiny but normalizable quaternion keeps its meaning instead of taking the fallback."""
     c = _make_controller()
     ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
     ee_quat = torch.tensor([_quat_xyzw([1.0, 0.0, 0.0], 0.5)])  # a fallback that is NOT identity
@@ -117,67 +119,6 @@ def test_set_command_tiny_nonzero_quat_is_still_normalized(scale):
     torch.testing.assert_close(c.ee_quat_des, torch.tensor([_ID_QUAT]), atol=1e-5, rtol=0.0)
 
 
-def test_set_command_underflowing_quat_norm_takes_fallback():
-    """A quaternion whose norm underflows to zero cannot be normalized, so it takes the fallback."""
-    c = _make_controller()
-    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
-    ee_quat = torch.tensor([_quat_xyzw([1.0, 0.0, 0.0], 0.5)])
-    cmd = torch.cat([ee_pos, torch.tensor([[0.0, 0.0, 0.0, 1e-38]])], dim=-1)
-    c.set_command(cmd, ee_pos, ee_quat)
-    assert torch.isfinite(c.ee_quat_des).all()
-    torch.testing.assert_close(c.ee_quat_des, ee_quat, atol=1e-6, rtol=0.0)
-
-
-def test_set_command_zero_quat_without_current_orientation_is_identity():
-    """Without a current orientation to hold, a zero-norm command falls back to identity."""
-    c = _make_controller()
-    cmd = torch.cat([torch.tensor([[0.3, -0.1, 0.2]]), torch.zeros(1, 4)], dim=-1)
-    c.set_command(cmd)
-    torch.testing.assert_close(c.ee_quat_des, torch.tensor([_ID_QUAT]), atol=1e-6, rtol=0.0)
-
-
-def test_set_command_zero_quat_only_affects_degenerate_envs():
-    """The fallback is per-environment: a valid command alongside a degenerate one is untouched."""
-    c = _make_controller(num_envs=2)
-    good_quat = _quat_xyzw([0.0, 1.0, 0.0], 0.4)
-    ee_pos = torch.tensor([[0.3, -0.1, 0.2], [0.3, -0.1, 0.2]])
-    ee_quat = torch.tensor([_ID_QUAT, _ID_QUAT])
-    cmd = torch.tensor([[0.3, -0.1, 0.2] + good_quat, [0.3, -0.1, 0.2, 0.0, 0.0, 0.0, 0.0]])
-    c.set_command(cmd, ee_pos, ee_quat)
-    torch.testing.assert_close(c.ee_quat_des[0], torch.tensor(good_quat), atol=1e-6, rtol=0.0)
-    torch.testing.assert_close(c.ee_quat_des[1], torch.tensor(_ID_QUAT), atol=1e-6, rtol=0.0)
-
-
-@pytest.mark.parametrize("bad_value", [float("nan"), float("inf"), float("-inf")])
-def test_adaptive_dls_reports_non_finite_jacobian_cause(bad_value):
-    """A non-finite Jacobian raises an error naming the real cause, not "matrix is singular".
-
-    The check is unconditional rather than a reaction to a decomposition error: backends differ in
-    whether ``svdvals``/``solve`` raise on non-finite input or propagate NaN silently, and a silent
-    NaN would otherwise reach the joint position targets.
-    """
-    c = _make_controller(ik_method="adaptive_dls")
-    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
-    ee_quat = torch.tensor([_ID_QUAT])
-    c.set_command(torch.tensor([[0.31, -0.1, 0.2] + _ID_QUAT]), ee_pos, ee_quat)
-    # otherwise well-conditioned, so only the non-finite entry can trigger the failure
-    jac = torch.eye(6, _NUM_JOINTS).unsqueeze(0).clone()
-    jac[0, 0, 0] = bad_value
-    with pytest.raises(RuntimeError, match="non-finite Jacobian"):
-        c.compute(ee_pos, ee_quat, jac, torch.zeros(1, _NUM_JOINTS))
-
-
-def test_adaptive_dls_finite_jacobian_is_unaffected_by_the_guard():
-    """The non-finite guard does not change results for a well-conditioned Jacobian."""
-    c = _make_controller(ik_method="adaptive_dls")
-    ee_pos = torch.tensor([[0.3, -0.1, 0.2]])
-    ee_quat = torch.tensor([_ID_QUAT])
-    c.set_command(torch.tensor([[0.31, -0.1, 0.2] + _ID_QUAT]), ee_pos, ee_quat)
-    jac = torch.eye(6, _NUM_JOINTS).unsqueeze(0).clone()
-    out = c.compute(ee_pos, ee_quat, jac, torch.zeros(1, _NUM_JOINTS))
-    assert torch.isfinite(out).all()
-
-
 def test_orientation_weight_none_is_unweighted():
     """With no orientation weight, the pose task Jacobian equals the raw Jacobian."""
     c = _make_controller(orientation_weight=None)