From aabe415af02c0fe9336fd9eb3693e962b5b577e2 Mon Sep 17 00:00:00 2001 From: Taha Zarif Date: Wed, 9 Sep 2026 18:06:00 +0430 Subject: [PATCH 1/2] fix: check IK convergence before solver step --- src/roboticstoolbox/robot/IK.py | 48 ++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 22 deletions(-) diff --git a/src/roboticstoolbox/robot/IK.py b/src/roboticstoolbox/robot/IK.py index 9fafc0837..48fcd87c6 100644 --- a/src/roboticstoolbox/robot/IK.py +++ b/src/roboticstoolbox/robot/IK.py @@ -293,36 +293,21 @@ def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution: linalg_error = 0 # Initialise variables - E = 0.0 + E = np.inf q = q0[0] for search in range(self.slimit): q = q0[search].copy() i = 0 - while i < self.ilimit: - i += 1 - - # step() reports E for q as it was *before* this iteration's - # update. An undamped update (GN/NR) can overshoot, so if E is - # already below tol we must return this pre-step q, not the - # mutated one step() hands back - otherwise we can report - # success with a q whose actual residual is far above tol. - q_prev = q.copy() - - # Attempt a step - try: - E, q[ets.jindices] = self.step(ets, Tep, q) - - except np.linalg.LinAlgError: - # Abandon search and try again - linalg_error += 1 - break + # Check the initial configuration before attempting an update. + # An exact or already-converged q0 must not enter a solver step, + # which can be singular even though the requested pose is solved. + _, E = self.error(ets.eval(q), Tep) - # Check if we have arrived + while True: + # Check convergence for the current q before another update. if E < self.tol: - q = q_prev - # Wrap q to be within +- 180 deg # If your robot has larger than 180 deg range on a joint # this line should be modified in incorporate the extra range @@ -344,6 +329,21 @@ def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution: residual=E, reason="Success", ) + + if i >= self.ilimit: + break + + i += 1 + + # Attempt a step. step() reports E for the updated q. + try: + E, q[ets.jindices] = self.step(ets, Tep, q) + + except np.linalg.LinAlgError: + # Abandon search and try again + linalg_error += 1 + break + total_i += i # If we make it here, then we have failed @@ -709,6 +709,7 @@ def step( else: q[ets.jindices] += np.linalg.inv(J) @ e + qnull + _, E = self.error(ets.eval(q), Tep) return E, q[ets.jindices] @@ -941,6 +942,7 @@ def step(self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray): q[ets.jindices] += np.linalg.inv(J.T @ self.We @ J + Wn) @ g + qnull + _, E = self.error(ets.eval(q), Tep) return E, q[ets.jindices] @@ -1121,6 +1123,7 @@ def step( else: q[ets.jindices] += np.linalg.inv(J) @ e + qnull + _, E = self.error(ets.eval(q), Tep) return E, q[ets.jindices] @@ -1388,6 +1391,7 @@ def step( q += xd[: ets.n] + _, E = self.error(ets.eval(q), Tep) return E, q From 8b0eb83b0540ea003905e013873a1ea19028558c Mon Sep 17 00:00:00 2001 From: Taha Zarif Date: Wed, 9 Sep 2026 18:06:16 +0430 Subject: [PATCH 2/2] test: cover exact IK initial solution --- tests/test_IK.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/tests/test_IK.py b/tests/test_IK.py index 30c3dc06b..1672ec4f5 100644 --- a/tests/test_IK.py +++ b/tests/test_IK.py @@ -205,6 +205,26 @@ def test_IK_LM1(self): self.assertGreater(test_tol, E) + def test_exact_q0_converges_without_solver_step(self): + panda = rtb.models.Panda().ets() + q0 = np.array([0.0, -0.3, 0.0, -2.2, 0.0, 2.0, np.pi / 4]) + Tep = panda.eval(q0) + + solvers = ( + rtb.IK_LM(method="chan", slimit=1), + rtb.IK_NR(pinv=False, slimit=1), + rtb.IK_GN(pinv=False, slimit=1), + ) + + for solver in solvers: + with self.subTest(solver=solver.name): + sol = solver.solve(panda, Tep, q0=q0) + + self.assertEqual(sol.success, True) + self.assertEqual(sol.iterations, 0) + self.assertLess(sol.residual, solver.tol) + nt.assert_allclose(sol.q, q0, atol=1e-12) + def test_IK_LM2(self): tol = 1e-6