fix(ik): _random_q() silently produces garbage for non-finite joint limits (#648)

* fix(ik): _random_q() silently produces garbage for non-finite joint limits

_random_q() (used by every numeric IK solver -- IK_NR/IK_GN/IK_LM/IK_QP --
to seed random restarts) sampled directly from a joint's qlim with no
check that the limits were actually finite. A joint with a bad
(non-finite, e.g. inf/-inf) limit baked into its model's own data --
this was the real root cause behind #485's KinovaGen3 report, whose
own qlim was fixed separately without ever patching this gap -- caused
either a silent NaN joint value or an opaque internal numpy error
(OverflowError: high - low range exceeds valid bounds, depending on
which RNG code path is hit), instead of a clear diagnostic pointing at
the actual bad joint.

Now raises a ValueError naming the offending joint index(es) and their
bad qlim before sampling, matching the existing convention elsewhere in
this code (e.g. ETS.qlim already raises for an unset prismatic limit)
of failing loudly rather than propagating a silent bad value.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>

* fix(ik): apply the same non-finite-qlim guard to the C++ IK fast path

ik.py's _random_q() (fixed in the previous commit) and ik.cpp's own
_rand_q() are separate implementations reached by different public
entry points -- ikine_LM/ikine_NR/ikine_GN/ikine_QP go through the
pure-Python IK_LM/IK_NR/IK_GN/IK_QP classes, while ik_LM/ik_NR/ik_GN
(documented as "a fast solver implemented in C++") go through
ik.cpp via nanobind. The C++ side had the identical gap with no
guard at all: sampling a non-finite qlim via raw Eigen arithmetic
silently produced a NaN q, which the solve loop then burned through
every one of its random restarts on before returning a "failed"
solution containing NaN -- no exception, no diagnostic.

Since RTB's public API is "IK" regardless of which implementation
backs a given method name, both solvers now enforce the same
contract: _rand_q() throws nb::value_error (mapping to a Python
ValueError, matching the Python-side message) before sampling if
any joint's qlim is non-finite.

Verified by rebuilding the compiled extension locally and confirming
the fail-then-pass behavior directly: pre-fix, ets.ik_LM() silently
returned (q=[nan], success=0) after 101 wasted searches; post-fix,
it raises ValueError immediately.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>

---------

Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>

Author: petercorke

src/roboticstoolbox/ets/cpp-extensions/ik.cpp

@@ -7,9 +7,13 @@
 
 #include <Python.h>
 #include <math.h>
+#include <cmath>
 #include <iostream>
 #include <functional>
 #include <Eigen/Dense>
+#include <nanobind/nanobind.h>
+
+namespace nb = nanobind;
 
 // ---------------------------------------------------------------------------
 // Shared loop kernel — all five IK solvers use this.
@@ -290,6 +294,19 @@ extern "C"
         Eigen::Map<Eigen::ArrayXd> qlim_l(ets->qlim_l, ets->n);
         Eigen::Map<Eigen::ArrayXd> q_range2(ets->q_range2, ets->n);
 
+        // A joint with a non-finite limit (inf/-inf/NaN, typically a bad
+        // value baked into a robot model's own joint-limit data) would
+        // otherwise silently propagate inf/NaN into q below, with no
+        // diagnostic at all -- mirrors the equivalent check in the
+        // pure-Python solver path (IK.py's _random_q()).
+        for (int i = 0; i < ets->n; i++)
+        {
+            if (!std::isfinite(qlim_l(i)) || !std::isfinite(q_range2(i)))
+                throw nb::value_error(
+                    "Joint limit(s) are not finite -- can't generate a "
+                    "random configuration within an infinite/undefined range.");
+        }
+
         q = VectorX::Random(ets->n);
 
         q = (q.array() + 1) * q_range2;

src/roboticstoolbox/robot/IK.py

@@ -406,24 +406,35 @@ def _random_q(self, ets: "rtb.ETS", i: int = 1) -> np.ndarray:
         :returns: An ``i x n`` ndarray of random valid joint configurations, where n
             is the number of joints in the ``ets``
         :rtype: numpy.ndarray
+        :raises ValueError: a joint's qlim is not finite (e.g. ``inf``/``-inf``
+            or ``NaN``, typically from a bad value in a robot model's own
+            joint-limit data)
 
         Generates a random q vector within the joint limits defined by ``ets.qlim``.
         """
 
+        qlim = ets.qlim
+
+        if not np.all(np.isfinite(qlim)):
+            bad = np.flatnonzero(~np.all(np.isfinite(qlim), axis=0))
+            raise ValueError(
+                f"Joint limit(s) for joint index(es) {bad.tolist()} are not "
+                f"finite (qlim={qlim[:, bad].tolist()}) -- can't generate a "
+                "random configuration within an infinite/undefined range."
+            )
+
         if i == 1:
             q = np.zeros((1, ets.n))
 
             for i in range(ets.n):
-                q[0, i] = self._private_random.uniform(ets.qlim[0, i], ets.qlim[1, i])
+                q[0, i] = self._private_random.uniform(qlim[0, i], qlim[1, i])
 
         else:
             q = np.zeros((i, ets.n))
 
             for j in range(i):
                 for i in range(ets.n):
-                    q[j, i] = self._private_random.uniform(
-                        ets.qlim[0, i], ets.qlim[1, i]
-                    )
+                    q[j, i] = self._private_random.uniform(qlim[0, i], qlim[1, i])
 
         return q
 

tests/test_IK.py

@@ -846,6 +846,37 @@ def test_iter_iksol(self):
         self.assertEqual(e, 0.1)
         self.assertEqual(f, "")
 
+    def test_random_q_rejects_non_finite_qlim(self):
+        # _random_q() used to sample straight from ets.qlim with no check --
+        # a joint with a bad (non-finite) limit baked into its model data
+        # would silently produce garbage (NaN, or an opaque numpy internal
+        # error) instead of a clear diagnostic. A finite joint's random_q
+        # should be unaffected.
+        et = rtb.ET.Rz(qlim=[-np.inf, np.inf])
+        ets = rtb.ETS([et])
+        solver = rtb.IK_LM()
+
+        with self.assertRaises(ValueError):
+            solver._random_q(ets, 1)
+
+        good_et = rtb.ET.Rz(qlim=[-np.pi, np.pi])
+        good_ets = rtb.ETS([good_et])
+        q = solver._random_q(good_ets, 5)
+        self.assertTrue(np.all(np.isfinite(q)))
+        self.assertEqual(q.shape, (5, 1))
+
+    def test_ik_lm_c_rejects_non_finite_qlim(self):
+        # Same guard, mirrored in the compiled fast-path solver (ets.ik_LM(),
+        # backed by ik.cpp's own _rand_q()) -- this is a genuinely separate
+        # implementation from IK_LM/_random_q() above, and used to silently
+        # return a NaN "solution" (success=0) after burning through every
+        # random restart, rather than raising.
+        et = rtb.ET.Rz(qlim=[-np.inf, np.inf])
+        ets = rtb.ETS([et])
+
+        with self.assertRaises(ValueError):
+            ets.ik_LM(np.eye(4))
+
 
 if __name__ == "__main__":
     unittest.main()