IK solvers perform unnecessary and dangerous last update (can return success=False even when valid solution was found)

[niamorg]

RTB version: all versions

Description

The numerical IK solvers can perform an unnecessary, and potentially invalid, iteration when the initial joint configuration already satisfies the convergence criterion.

All solvers (IK_LM, IK_NR, IK_GN, IK_QP) share the same IKSolver._solve() loop. The current control flow is effectively:

1. _solve() saves current configuration q as q_prev: q_prev = q.copy()
2. step() computes the current error criterion E: E = error(q)
3. step() performs the numerical update (q is updated).
4. _solve() checks whether E < tol and if so it returns the configuration q_prev.

Therefore the step is performed even if stopping criterion is true...

This is particularly problematic for IK_LM(method="chan"). At an exact solution, E is approximately zero, so during the step Chan’s damping matrix becomes: Wn = k * E * np.eye(ets.n) ≈ 0 and the update then attempts to invert J.T @ self.We @ J + Wn which may be singular or badly conditioned (if robot has more than 6 DoFs). Therefore, a solver can fail even though the current q satisfies the requested pose (see sample code).

The previous fix is incomplete

Commit 3606c3b added the q_prev variable to avoid returning the post-step q (since it is the pre-step configuration that is associated with the stopping criterion).

It does not fix the underlying control-flow issue (which is arguably easier to fix):

• the unnecessary step is still performed
• matrix inversion can still fail and raise an exception, which causes the returned solution to have success=False

Suggested fix

• Initalize error E with error(q0) instead of E = 0.
• Move criterion check (step 4) before call to step() (steps 2 and 3)
• Make step() return error(post-update q) instead of error(pre-update q) (i.e. swap step 2 and 3)
• Remove previous fix (i.e. remove q_prev variable): delete step 1. and make step 4 return q not q_prev.

Minimal reproducible example

# Solver returns success=False when exact joint config is given as q0.

import numpy as np
import roboticstoolbox as rtb

robot = rtb.models.Panda().ets()  # 7 DoFs robot
q = np.zeros(7)
T = robot.fkine(q).A

solution = rtb.IK_LM(method="chan", slimit=1).solve(robot, T, q0=q)
print(solution)
# IKSolution: q=[0, 0, 0, 0, 0, 0, 0], success=False, reason=iteration and search limit reached, 1 numpy.LinAlgError encountered, iterations=1, searches=1, residual=0