From 4c2c851824736539e8fdca4a5f511d275b25f091 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 26 Aug 2026 14:29:58 +1000 Subject: [PATCH 1/2] 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 --- src/roboticstoolbox/robot/IK.py | 19 +++++++++++++++---- tests/test_IK.py | 19 +++++++++++++++++++ 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index 075412b3c..15155e7fe 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/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 diff --git a/tests/test_IK.py b/tests/test_IK.py index 7c5858fef..2f0d96ce1 100644 --- a/tests/test_IK.py +++ b/tests/test_IK.py @@ -846,6 +846,25 @@ 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)) + if __name__ == "__main__": unittest.main() From 562566f5aff1f6f040d6409bc7ba8b4695c17cee Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Wed, 26 Aug 2026 14:49:34 +1000 Subject: [PATCH 2/2] 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 --- src/roboticstoolbox/ets/cpp-extensions/ik.cpp | 17 +++++++++++++++++ tests/test_IK.py | 12 ++++++++++++ 2 files changed, 29 insertions(+) diff --git a/src/roboticstoolbox/ets/cpp-extensions/ik.cpp b/src/roboticstoolbox/ets/cpp-extensions/ik.cpp index 937fa3bad..8196ac455 100644 --- a/src/roboticstoolbox/ets/cpp-extensions/ik.cpp +++ b/src/roboticstoolbox/ets/cpp-extensions/ik.cpp @@ -7,9 +7,13 @@ #include #include +#include #include #include #include +#include + +namespace nb = nanobind; // --------------------------------------------------------------------------- // Shared loop kernel — all five IK solvers use this. @@ -290,6 +294,19 @@ extern "C" Eigen::Map qlim_l(ets->qlim_l, ets->n); Eigen::Map 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; diff --git a/tests/test_IK.py b/tests/test_IK.py index 2f0d96ce1..412aaad59 100644 --- a/tests/test_IK.py +++ b/tests/test_IK.py @@ -865,6 +865,18 @@ def test_random_q_rejects_non_finite_qlim(self): 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()