Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions src/roboticstoolbox/robot/IK.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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"

Expand Down
65 changes: 65 additions & 0 deletions tests/test_IK.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Loading