From 3a32e46bfb0d474e0659c32332b9ea4b9cf54eac Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 17 Nov 2026 16:25:11 +1000 Subject: [PATCH] fix(ik): abandon a search whose joint angles diverge, and restart it A Gauss-Newton or Newton-Raphson search can diverge from a bad start. With the compiled fkine the infinite joint angles give NaN, and the search burned iterations until ilimit; with the pure-Python fkine, rotz(inf) raises ValueError: math domain error, which escaped solve() and ended the whole call. Which of test_IK_GN1 and test_IK_NR8 failed depended on the platform: a 1e-13 change to the target flipped the outcome. _solve() now abandons the search as soon as q or E is no longer finite, as it already does for LinAlgError, and says so in the failure reason. A ValueError with a finite q is still raised. step() updates q in place, so q shows which case it is. Co-Authored-By: Claude Sonnet 5.5 --- src/roboticstoolbox/robot/IK.py | 20 ++++++++++ tests/test_IK.py | 65 +++++++++++++++++++++++++++++++++ 2 files changed, 85 insertions(+) diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index cb0ba4f14..cc837fa24 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/src/roboticstoolbox/robot/IK.py @@ -479,6 +479,7 @@ def _solve(self, ets: ETS, Tep: NDArray, q0: NDArray) -> IKSolution: # Error flags found_with_limits = False linalg_error = 0 + diverged = 0 # Initialise variables E = np.inf @@ -547,6 +548,22 @@ def _solve(self, ets: ETS, Tep: NDArray, q0: NDArray) -> IKSolution: self._log("Abandoned", E, "numpy.linalg.LinAlgError") break + except ValueError: + # step() updates q in place, so q shows what went wrong. If + # it is no longer finite the search has diverged, and the + # pure-Python fkine raises where the compiled one returns + # NaN. Anything else is a genuine error. + if np.all(np.isfinite(q[ets.jindices])): + raise + E = np.inf + + if not np.isfinite(E): + # Abandon search and try again, there is no way back from NaN + diverged += 1 + if self.verbose: + self._log("Abandoned", E, "search diverged") + break + total_i += i # If we make it here, then we have failed @@ -555,6 +572,9 @@ def _solve(self, ets: ETS, Tep: NDArray, q0: NDArray) -> IKSolution: if linalg_error: reason += f", {linalg_error} numpy.LinAlgError encountered" + if diverged: + reason += f", {diverged} searches diverged" + if found_with_limits: reason += ", solution found but violates joint limits" diff --git a/tests/test_IK.py b/tests/test_IK.py index 852597ade..3a0c8ac28 100644 --- a/tests/test_IK.py +++ b/tests/test_IK.py @@ -1441,5 +1441,70 @@ def test_cpp_solvers_do_not_take_it(self): getattr(obj, name)(Tep, verbose=True) +class TestDivergedSearch(unittest.TestCase): + """ + A search whose q diverges to infinity is abandoned and restarted. + + Gauss-Newton and its relatives can diverge from a bad start. The compiled + fkine returns NaN for an infinite joint angle but the pure-Python one + raises ValueError, and either used to end the whole solve. + """ + + def setUp(self): + self.ets = rtb.models.Panda().ets() + self.Tep = self.ets.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + def _diverge_first_step(self, solver, raises): + step = solver.step + calls = [] + + def diverging(ets, Tep, q): + calls.append(1) + if len(calls) == 1: + q[ets.jindices] = np.inf + if raises: + raise ValueError("math domain error") + return np.nan, q[ets.jindices] + return step(ets, Tep, q) + + solver.step = diverging + + def test_pure_python_fkine_raises(self): + solver = rtb.IK_LM(seed=0) + self._diverge_first_step(solver, raises=True) + sol = solver.solve(self.ets, self.Tep) + self.assertTrue(sol.success) + self.assertGreaterEqual(sol.searches, 2) + + def test_compiled_fkine_returns_nan(self): + solver = rtb.IK_LM(seed=0) + self._diverge_first_step(solver, raises=False) + sol = solver.solve(self.ets, self.Tep) + self.assertTrue(sol.success) + self.assertGreaterEqual(sol.searches, 2) + + def test_failure_says_why(self): + solver = rtb.IK_LM(seed=0, slimit=3) + + def diverging(ets, Tep, q): + q[ets.jindices] = np.inf + raise ValueError("math domain error") + + solver.step = diverging + sol = solver.solve(self.ets, self.Tep) + self.assertFalse(sol.success) + self.assertIn("3 searches diverged", sol.reason) + + def test_other_value_errors_propagate(self): + solver = rtb.IK_LM(seed=0) + + def broken(ets, Tep, q): + raise ValueError("not a divergence") + + solver.step = broken + with self.assertRaisesRegex(ValueError, "not a divergence"): + solver.solve(self.ets, self.Tep) + + if __name__ == "__main__": unittest.main()