FA-87926 / Inverse kinematics solvers / Open access
Step limiter clips joints independently and bends the step direction · case 01
Large corrections change direction instead of being scaled along the DLS direction.
ROOT CAUSE
Each joint delta is clamped to +/-maxstep separately instead of scaling the whole vector.
VERIFIED REPAIR
Scale both components by maxstep/||dq|| when the norm exceeds maxstep.
Unsuccessful approach: Multiplying by maxstep without dividing by the norm rescales every step, even small ones, incorrectly.
Case contract
Input [l1,l2,q1,q2,tx,ty,lam,maxstep], angles in degrees, maxstep in radians. Perform one damped least squares step dq = J^T (J J^T + lam^2 I)^-1 e with e = target - fk(q), scale dq so its Euclidean norm is at most maxstep, and return the updated [q1,q2] in degrees rounded to 4 places; a near-singular damped matrix (|det|<1e-12) returns "singular".
Why this case matters
Inverse kinematics code turns task-space goals into joint commands; a wrong branch, sign, limit or update order sends a real arm to the wrong pose.
1 / The failure
Exit 1"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
l1,l2,q1,q2,tx,ty,lam,maxstep=x
a=math.radians(q1)
b=math.radians(q1+q2)
ex=tx-(l1*math.cos(a)+l2*math.cos(b))
ey=ty-(l1*math.sin(a)+l2*math.sin(b))
j11=-l1*math.sin(a)-l2*math.sin(b)
j12=-l2*math.sin(b)
j21=l1*math.cos(a)+l2*math.cos(b)
j22=l2*math.cos(b)
m11=j11*j11+j12*j12+lam*lam
m12=j11*j21+j12*j22
m22=j21*j21+j22*j22+lam*lam
det=m11*m22-m12*m12
if abs(det)<1e-12: return 'singular'
w1=(m22*ex-m12*ey)/det
w2=(-m12*ex+m11*ey)/det
d1=j11*w1+j21*w2
d2=j12*w1+j22*w2
n=math.hypot(d1,d2)
d1=max(-maxstep,min(maxstep,d1)); d2=max(-maxstep,min(maxstep,d2))
return [round(q1+math.degrees(d1),4),round(q2+math.degrees(d2),4)]
def check(label, actual, expected):
observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['straight arm damped', [[1, 1, 0, 0, 1.5, 0.5, 0.5, 1.0]], [10.9135, 5.4567]], ['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['already on target', [[1, 1, 0, 90, 1, 1, 0.1, 1.0]], [0.0, 90.0]], ['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]]], [['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['already on target', [[1, 1, 0, 90, 1, 1, 0.1, 1.0]], [0.0, 90.0]], ['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]], ['tiny step budget', [[1, 1, 10, 20, 0, 1.8, 0.05, 0.01]], [9.7971, 20.5358]], ['long upper arm', [[3, 1, 60, -30, 1, 3, 0.3, 0.3]], [66.6639, -45.8444]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]]], [['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]], ['tiny step budget', [[1, 1, 10, 20, 0, 1.8, 0.05, 0.01]], [9.7971, 20.5358]], ['long upper arm', [[3, 1, 60, -30, 1, 3, 0.3, 0.3]], [66.6639, -45.8444]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['mid budget', [[2, 2, -120, 60, -1, -2, 0.1, 0.4]], [-133.1333, 78.7821]], ['upper reach', [[1, 2, 90, -45, -1, 2.5, 0.05, 1.5]], [151.7219, -104.8057]]], [['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['mid budget', [[2, 2, -120, 60, -1, -2, 0.1, 0.4]], [-133.1333, 78.7821]], ['upper reach', [[1, 2, 90, -45, -1, 2.5, 0.05, 1.5]], [151.7219, -104.8057]], ['unit damping', [[1, 1, 15, 15, 0.2, 1.9, 1.0, 1.0]], [44.712, 37.4827]]], [['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['straight arm damped', [[1, 1, 0, 0, 1.5, 0.5, 0.5, 1.0]], [10.9135, 5.4567]], ['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['unit damping', [[1, 1, 15, 15, 0.2, 1.9, 1.0, 1.0]], [44.712, 37.4827]]]]
for label, args, expected in fixtures[N-1]:
check(label, solve(*args), expected)
print(json.dumps({"observations": observations, "passed": all(x["passed"] for x in observations)}, ensure_ascii=False))
raise SystemExit(0 if all(x["passed"] for x in observations) else 1)
| Boundary fixture | Actual | Expected | Outcome |
|---|---|---|---|
| small correction | [22.9688, 62.7571] | [22.9688, 62.7571] | Passed |
| large correction clamped | [-11.4592, 101.4592] | [-1.7958, 101.3176] | Failed |
| straight arm damped | [10.9135, 5.4567] | [10.9135, 5.4567] | Passed |
| undamped singular | singular | singular | Passed |
| heavy damping | [31.2311, 68.0501] | [31.2311, 68.0501] | Passed |
| already on target | [0.0, 90.0] | [0.0, 90.0] | Passed |
| negative angles | [-22.919, -92.471] | [-22.919, -92.471] | Passed |
SHA-256 / 803e011b4794c1afd4202db7bf362211412be8bc7e29ace28ac08eb608ac5578
2 / The unsuccessful fix
Exit 1"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
l1,l2,q1,q2,tx,ty,lam,maxstep=x
a=math.radians(q1)
b=math.radians(q1+q2)
ex=tx-(l1*math.cos(a)+l2*math.cos(b))
ey=ty-(l1*math.sin(a)+l2*math.sin(b))
j11=-l1*math.sin(a)-l2*math.sin(b)
j12=-l2*math.sin(b)
j21=l1*math.cos(a)+l2*math.cos(b)
j22=l2*math.cos(b)
m11=j11*j11+j12*j12+lam*lam
m12=j11*j21+j12*j22
m22=j21*j21+j22*j22+lam*lam
det=m11*m22-m12*m12
if abs(det)<1e-12: return 'singular'
w1=(m22*ex-m12*ey)/det
w2=(-m12*ex+m11*ey)/det
d1=j11*w1+j21*w2
d2=j12*w1+j22*w2
n=math.hypot(d1,d2)
if n>maxstep and n>0: d1,d2=d1*maxstep,d2*maxstep
return [round(q1+math.degrees(d1),4),round(q2+math.degrees(d2),4)]
def check(label, actual, expected):
observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['straight arm damped', [[1, 1, 0, 0, 1.5, 0.5, 0.5, 1.0]], [10.9135, 5.4567]], ['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['already on target', [[1, 1, 0, 90, 1, 1, 0.1, 1.0]], [0.0, 90.0]], ['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]]], [['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['already on target', [[1, 1, 0, 90, 1, 1, 0.1, 1.0]], [0.0, 90.0]], ['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]], ['tiny step budget', [[1, 1, 10, 20, 0, 1.8, 0.05, 0.01]], [9.7971, 20.5358]], ['long upper arm', [[3, 1, 60, -30, 1, 3, 0.3, 0.3]], [66.6639, -45.8444]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]]], [['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]], ['tiny step budget', [[1, 1, 10, 20, 0, 1.8, 0.05, 0.01]], [9.7971, 20.5358]], ['long upper arm', [[3, 1, 60, -30, 1, 3, 0.3, 0.3]], [66.6639, -45.8444]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['mid budget', [[2, 2, -120, 60, -1, -2, 0.1, 0.4]], [-133.1333, 78.7821]], ['upper reach', [[1, 2, 90, -45, -1, 2.5, 0.05, 1.5]], [151.7219, -104.8057]]], [['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['mid budget', [[2, 2, -120, 60, -1, -2, 0.1, 0.4]], [-133.1333, 78.7821]], ['upper reach', [[1, 2, 90, -45, -1, 2.5, 0.05, 1.5]], [151.7219, -104.8057]], ['unit damping', [[1, 1, 15, 15, 0.2, 1.9, 1.0, 1.0]], [44.712, 37.4827]]], [['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['straight arm damped', [[1, 1, 0, 0, 1.5, 0.5, 0.5, 1.0]], [10.9135, 5.4567]], ['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['unit damping', [[1, 1, 15, 15, 0.2, 1.9, 1.0, 1.0]], [44.712, 37.4827]]]]
for label, args, expected in fixtures[N-1]:
check(label, solve(*args), expected)
print(json.dumps({"observations": observations, "passed": all(x["passed"] for x in observations)}, ensure_ascii=False))
raise SystemExit(0 if all(x["passed"] for x in observations) else 1)
| Boundary fixture | Actual | Expected | Outcome |
|---|---|---|---|
| small correction | [22.9688, 62.7571] | [22.9688, 62.7571] | Passed |
| large correction clamped | [-5.3397, 123.6511] | [-1.7958, 101.3176] | Failed |
| straight arm damped | [10.9135, 5.4567] | [10.9135, 5.4567] | Passed |
| undamped singular | singular | singular | Passed |
| heavy damping | [31.2311, 68.0501] | [31.2311, 68.0501] | Passed |
| already on target | [0.0, 90.0] | [0.0, 90.0] | Passed |
| negative angles | [-22.919, -92.471] | [-22.919, -92.471] | Passed |
SHA-256 / af828c09f63bee39684c126704e0e0ff53c907465040c12a80059885e9727a7f
3 / The verified repair
Exit 0"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
l1,l2,q1,q2,tx,ty,lam,maxstep=x
a=math.radians(q1)
b=math.radians(q1+q2)
ex=tx-(l1*math.cos(a)+l2*math.cos(b))
ey=ty-(l1*math.sin(a)+l2*math.sin(b))
j11=-l1*math.sin(a)-l2*math.sin(b)
j12=-l2*math.sin(b)
j21=l1*math.cos(a)+l2*math.cos(b)
j22=l2*math.cos(b)
m11=j11*j11+j12*j12+lam*lam
m12=j11*j21+j12*j22
m22=j21*j21+j22*j22+lam*lam
det=m11*m22-m12*m12
if abs(det)<1e-12: return 'singular'
w1=(m22*ex-m12*ey)/det
w2=(-m12*ex+m11*ey)/det
d1=j11*w1+j21*w2
d2=j12*w1+j22*w2
n=math.hypot(d1,d2)
if n>maxstep and n>0: d1,d2=d1*maxstep/n,d2*maxstep/n
return [round(q1+math.degrees(d1),4),round(q2+math.degrees(d2),4)]
def check(label, actual, expected):
observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['straight arm damped', [[1, 1, 0, 0, 1.5, 0.5, 0.5, 1.0]], [10.9135, 5.4567]], ['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['already on target', [[1, 1, 0, 90, 1, 1, 0.1, 1.0]], [0.0, 90.0]], ['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]]], [['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['already on target', [[1, 1, 0, 90, 1, 1, 0.1, 1.0]], [0.0, 90.0]], ['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]], ['tiny step budget', [[1, 1, 10, 20, 0, 1.8, 0.05, 0.01]], [9.7971, 20.5358]], ['long upper arm', [[3, 1, 60, -30, 1, 3, 0.3, 0.3]], [66.6639, -45.8444]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]]], [['negative angles', [[1.5, 1, -40, -70, 1, -1.5, 0.2, 0.5]], [-22.919, -92.471]], ['tiny step budget', [[1, 1, 10, 20, 0, 1.8, 0.05, 0.01]], [9.7971, 20.5358]], ['long upper arm', [[3, 1, 60, -30, 1, 3, 0.3, 0.3]], [66.6639, -45.8444]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['mid budget', [[2, 2, -120, 60, -1, -2, 0.1, 0.4]], [-133.1333, 78.7821]], ['upper reach', [[1, 2, 90, -45, -1, 2.5, 0.05, 1.5]], [151.7219, -104.8057]]], [['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['target behind', [[1, 1, 45, 45, -1.2, -0.4, 0.2, 2.0]], [2.6992, 151.4982]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['mid budget', [[2, 2, -120, 60, -1, -2, 0.1, 0.4]], [-133.1333, 78.7821]], ['upper reach', [[1, 2, 90, -45, -1, 2.5, 0.05, 1.5]], [151.7219, -104.8057]], ['unit damping', [[1, 1, 15, 15, 0.2, 1.9, 1.0, 1.0]], [44.712, 37.4827]]], [['small correction', [[1, 1, 30, 45, 1.0, 1.4, 0.1, 1.0]], [22.9688, 62.7571]], ['large correction clamped', [[1, 1, 0, 90, -1.5, 0.5, 0.1, 0.2]], [-1.7958, 101.3176]], ['straight arm damped', [[1, 1, 0, 0, 1.5, 0.5, 0.5, 1.0]], [10.9135, 5.4567]], ['undamped singular', [[1, 1, 0, 0, 1.5, 0.5, 0.0, 1.0]], 'singular'], ['heavy damping', [[2, 1, 20, 60, 1, 2, 2.0, 1.0]], [31.2311, 68.0501]], ['lightly damped folded', [[1, 1, 30, 170, 0.5, 0.2, 0.01, 0.5]], [1.4116, 171.8446]], ['unit damping', [[1, 1, 15, 15, 0.2, 1.9, 1.0, 1.0]], [44.712, 37.4827]]]]
for label, args, expected in fixtures[N-1]:
check(label, solve(*args), expected)
print(json.dumps({"observations": observations, "passed": all(x["passed"] for x in observations)}, ensure_ascii=False))
raise SystemExit(0 if all(x["passed"] for x in observations) else 1)
| Boundary fixture | Actual | Expected | Outcome |
|---|---|---|---|
| small correction | [22.9688, 62.7571] | [22.9688, 62.7571] | Passed |
| large correction clamped | [-1.7958, 101.3176] | [-1.7958, 101.3176] | Passed |
| straight arm damped | [10.9135, 5.4567] | [10.9135, 5.4567] | Passed |
| undamped singular | singular | singular | Passed |
| heavy damping | [31.2311, 68.0501] | [31.2311, 68.0501] | Passed |
| already on target | [0.0, 90.0] | [0.0, 90.0] | Passed |
| negative angles | [-22.919, -92.471] | [-22.919, -92.471] | Passed |
SHA-256 / 14f9ed4a35a3690a31b7b2b6ee4c102bdf8754a76ab8ceea5f289f2c9a55ca79
Verification & scope
Deterministic planar or low-dimensional teaching model with a stipulated convention; not a general robotics library. This reproducer isolates one failure mechanism. Results cover the supplied fixtures. Variants within a family share a test contract and should remain grouped when constructing evaluation splits. Related mechanisms with a shared evaluation_group must also remain together; these controlled models are not independent production incidents.
Observations recorded using Python 3.12.14 at 2026-09-29T14:51:03.267099+00:00.
Case digest / 920c5b51fe9695b5eebc9cf78038a981239e99397a49c0bad13780d6b786fb6a