fix: check numerical IK convergence before solver step

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
 
 

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