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/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..412aaad59 100644 --- a/tests/test_IK.py +++ b/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()