FA-87901 / Inverse kinematics solvers / Open access

Reachability is judged on the tool tip with the tool length included · case 01

Targets whose wrist centre is out of reach return a clamped pose instead of "unreachable".

Verified by executionVariant 1 · 7 checks per implementationDownload source bundle ↓JSON ↗

ROOT CAUSE

The feasibility test compares the tip distance against l1+l2+l3, ignoring the required heading.

VERIFIED REPAIR

Test the wrist-centre distance against the two-link annulus.

Unsuccessful approach: Testing the wrist centre against l1+l2+l3 still admits wrist centres the sub-chain cannot reach.

Case contract

Input [l1,l2,l3,px,py,phi,elbow]: planar three-link arm, phi is the required tool heading in degrees. Subtract l3 along phi to get the wrist centre, solve the two-link sub-chain (elbow "up" gives q2>=0), set q3=phi-q1-q2 and wrap each joint to [-180,180) in degrees rounded to 4 places. A wrist centre outside |l1-l2|..l1+l2 returns "unreachable".

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,l3,px,py,phi,elbow=x
    a=math.radians(phi)
    wx=px-l3*math.cos(a)
    wy=py-l3*math.sin(a)
    r2=wx*wx+wy*wy
    if px*px+py*py>(l1+l2+l3)**2+1e-9 or r2<(l1-l2)**2-1e-9: return 'unreachable'
    c=(r2-l1*l1-l2*l2)/(2*l1*l2)
    c=max(-1.0,min(1.0,c))
    s=math.sqrt(1.0-c*c)*(1 if elbow=='up' else -1)
    q1=math.atan2(wy,wx)-math.atan2(l2*s,l1+l2*c)
    q2=math.atan2(s,c)
    q3=a-q1-q2
    out=[]
    for q in (q1,q2,q3):
        d=math.degrees(q)
        d=(d+180.0)%360.0-180.0
        out.append(round(d,4))
    return out
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['tool pointing along x', [[2, 1.5, 0.5, 3, 1, 0, 'up']], [-11.5172, 80.4059, -68.8887]], ['tool pointing up', [[2, 1.5, 0.5, 2, 2, 90, 'down']], [73.7398, -90.0, 106.2602]], ['tool pointing back', [[2, 2, 1, -1, 2, 180, 'up']], [30.0, 120.0, 30.0]], ['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['diagonal heading', [[2, 1, 1, 2, 2, 45, 'down']], [74.8541, -114.4698, 84.6157]], ['multi-turn heading', [[2, 1.5, 0.5, 1, 2.5, 405, 'up']], [32.3158, 101.7795, -89.0953]], ['negative heading', [[3, 2, 1, 3, -2, -30, 'down']], [5.9393, -121.0875, 85.1482]], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['multi-turn heading', [[2, 1.5, 0.5, 1, 2.5, 405, 'up']], [32.3158, 101.7795, -89.0953]], ['negative heading', [[3, 2, 1, 3, -2, -30, 'down']], [5.9393, -121.0875, 85.1482]], ['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]], ['zero length tool', [[2, 1, 0, 2, 1, 60, 'up']], [0.0, 90.0, -30.0]], ['heading 150', [[2, 1.5, 1, -1, 2.5, 150, 'down']], [137.8289, -111.8395, 124.0106]], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]], ['zero length tool', [[2, 1, 0, 2, 1, 60, 'up']], [0.0, 90.0, -30.0]], ['heading 150', [[2, 1.5, 1, -1, 2.5, 150, 'down']], [137.8289, -111.8395, 124.0106]], ['wrist in inner hole', [[3, 1, 1, 2.2, 0, 0, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable'], ['wrist stretched', [[2, 1, 1, 4, 0, 0, 'down']], [0.0, 0.0, 0.0]], ['heading -135', [[1.5, 1.5, 0.5, -1.2, -1.7, -135, 'down']], [-64.1702, -115.9708, 45.141]]], [['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable'], ['wrist stretched', [[2, 1, 1, 4, 0, 0, 'down']], [0.0, 0.0, 0.0]], ['heading -135', [[1.5, 1.5, 0.5, -1.2, -1.7, -135, 'down']], [-64.1702, -115.9708, 45.141]], ['near base with long tool', [[2, 2, 2, 0.5, 0.5, 10, 'up']], [105.7453, 136.645, 127.6097]], ['heading 300', [[2.5, 1.5, 0.8, 1, -3, 300, 'up']], [-111.095, 112.0608, -60.9658]], ['heading -300', [[2.5, 1.5, 0.8, 1, 3, -300, 'down']], [111.095, -112.0608, 60.9658]]]]
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
tool pointing along x	`[-11.5172, 80.4059, -68.8887]`	`[-11.5172, 80.4059, -68.8887]`	Passed
tool pointing up	`[73.7398, -90.0, 106.2602]`	`[73.7398, -90.0, 106.2602]`	Passed
tool pointing back	`[30.0, 120.0, 30.0]`	`[30.0, 120.0, 30.0]`	Passed
tool pointing down	`[-48.0197, 128.6822, -170.6625]`	`[-48.0197, 128.6822, -170.6625]`	Passed
wrist just reachable	`[-41.4096, 82.8192, -41.4096]`	`[-41.4096, 82.8192, -41.4096]`	Passed
tip in reach but wrist not	`[0.0, 0.0, -180.0]`	`unreachable`	Failed
wrist behind tip beyond reach	`[0.0, 0.0, -180.0]`	`unreachable`	Failed

SHA-256 / bc06a87f3ce5eaedefbe4121965ae839ee67c00e963a027aaceb61a415bda2ed

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,l3,px,py,phi,elbow=x
    a=math.radians(phi)
    wx=px-l3*math.cos(a)
    wy=py-l3*math.sin(a)
    r2=wx*wx+wy*wy
    if r2>(l1+l2+l3)**2+1e-9 or r2<(l1-l2)**2-1e-9: return 'unreachable'
    c=(r2-l1*l1-l2*l2)/(2*l1*l2)
    c=max(-1.0,min(1.0,c))
    s=math.sqrt(1.0-c*c)*(1 if elbow=='up' else -1)
    q1=math.atan2(wy,wx)-math.atan2(l2*s,l1+l2*c)
    q2=math.atan2(s,c)
    q3=a-q1-q2
    out=[]
    for q in (q1,q2,q3):
        d=math.degrees(q)
        d=(d+180.0)%360.0-180.0
        out.append(round(d,4))
    return out
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['tool pointing along x', [[2, 1.5, 0.5, 3, 1, 0, 'up']], [-11.5172, 80.4059, -68.8887]], ['tool pointing up', [[2, 1.5, 0.5, 2, 2, 90, 'down']], [73.7398, -90.0, 106.2602]], ['tool pointing back', [[2, 2, 1, -1, 2, 180, 'up']], [30.0, 120.0, 30.0]], ['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['diagonal heading', [[2, 1, 1, 2, 2, 45, 'down']], [74.8541, -114.4698, 84.6157]], ['multi-turn heading', [[2, 1.5, 0.5, 1, 2.5, 405, 'up']], [32.3158, 101.7795, -89.0953]], ['negative heading', [[3, 2, 1, 3, -2, -30, 'down']], [5.9393, -121.0875, 85.1482]], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['multi-turn heading', [[2, 1.5, 0.5, 1, 2.5, 405, 'up']], [32.3158, 101.7795, -89.0953]], ['negative heading', [[3, 2, 1, 3, -2, -30, 'down']], [5.9393, -121.0875, 85.1482]], ['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]], ['zero length tool', [[2, 1, 0, 2, 1, 60, 'up']], [0.0, 90.0, -30.0]], ['heading 150', [[2, 1.5, 1, -1, 2.5, 150, 'down']], [137.8289, -111.8395, 124.0106]], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]], ['zero length tool', [[2, 1, 0, 2, 1, 60, 'up']], [0.0, 90.0, -30.0]], ['heading 150', [[2, 1.5, 1, -1, 2.5, 150, 'down']], [137.8289, -111.8395, 124.0106]], ['wrist in inner hole', [[3, 1, 1, 2.2, 0, 0, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable'], ['wrist stretched', [[2, 1, 1, 4, 0, 0, 'down']], [0.0, 0.0, 0.0]], ['heading -135', [[1.5, 1.5, 0.5, -1.2, -1.7, -135, 'down']], [-64.1702, -115.9708, 45.141]]], [['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable'], ['wrist stretched', [[2, 1, 1, 4, 0, 0, 'down']], [0.0, 0.0, 0.0]], ['heading -135', [[1.5, 1.5, 0.5, -1.2, -1.7, -135, 'down']], [-64.1702, -115.9708, 45.141]], ['near base with long tool', [[2, 2, 2, 0.5, 0.5, 10, 'up']], [105.7453, 136.645, 127.6097]], ['heading 300', [[2.5, 1.5, 0.8, 1, -3, 300, 'up']], [-111.095, 112.0608, -60.9658]], ['heading -300', [[2.5, 1.5, 0.8, 1, 3, -300, 'down']], [111.095, -112.0608, 60.9658]]]]
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
tool pointing along x	`[-11.5172, 80.4059, -68.8887]`	`[-11.5172, 80.4059, -68.8887]`	Passed
tool pointing up	`[73.7398, -90.0, 106.2602]`	`[73.7398, -90.0, 106.2602]`	Passed
tool pointing back	`[30.0, 120.0, 30.0]`	`[30.0, 120.0, 30.0]`	Passed
tool pointing down	`[-48.0197, 128.6822, -170.6625]`	`[-48.0197, 128.6822, -170.6625]`	Passed
wrist just reachable	`[-41.4096, 82.8192, -41.4096]`	`[-41.4096, 82.8192, -41.4096]`	Passed
tip in reach but wrist not	`unreachable`	`unreachable`	Passed
wrist behind tip beyond reach	`[0.0, 0.0, -180.0]`	`unreachable`	Failed

SHA-256 / 1df9c5c9276123b4e9345ac205094bc618900e20a72fbeaf9d99cc5e63a7beaf

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,l3,px,py,phi,elbow=x
    a=math.radians(phi)
    wx=px-l3*math.cos(a)
    wy=py-l3*math.sin(a)
    r2=wx*wx+wy*wy
    if r2>(l1+l2)**2+1e-9 or r2<(l1-l2)**2-1e-9: return 'unreachable'
    c=(r2-l1*l1-l2*l2)/(2*l1*l2)
    c=max(-1.0,min(1.0,c))
    s=math.sqrt(1.0-c*c)*(1 if elbow=='up' else -1)
    q1=math.atan2(wy,wx)-math.atan2(l2*s,l1+l2*c)
    q2=math.atan2(s,c)
    q3=a-q1-q2
    out=[]
    for q in (q1,q2,q3):
        d=math.degrees(q)
        d=(d+180.0)%360.0-180.0
        out.append(round(d,4))
    return out
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['tool pointing along x', [[2, 1.5, 0.5, 3, 1, 0, 'up']], [-11.5172, 80.4059, -68.8887]], ['tool pointing up', [[2, 1.5, 0.5, 2, 2, 90, 'down']], [73.7398, -90.0, 106.2602]], ['tool pointing back', [[2, 2, 1, -1, 2, 180, 'up']], [30.0, 120.0, 30.0]], ['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['diagonal heading', [[2, 1, 1, 2, 2, 45, 'down']], [74.8541, -114.4698, 84.6157]], ['multi-turn heading', [[2, 1.5, 0.5, 1, 2.5, 405, 'up']], [32.3158, 101.7795, -89.0953]], ['negative heading', [[3, 2, 1, 3, -2, -30, 'down']], [5.9393, -121.0875, 85.1482]], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['multi-turn heading', [[2, 1.5, 0.5, 1, 2.5, 405, 'up']], [32.3158, 101.7795, -89.0953]], ['negative heading', [[3, 2, 1, 3, -2, -30, 'down']], [5.9393, -121.0875, 85.1482]], ['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]], ['zero length tool', [[2, 1, 0, 2, 1, 60, 'up']], [0.0, 90.0, -30.0]], ['heading 150', [[2, 1.5, 1, -1, 2.5, 150, 'down']], [137.8289, -111.8395, 124.0106]], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable']], [['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]], ['zero length tool', [[2, 1, 0, 2, 1, 60, 'up']], [0.0, 90.0, -30.0]], ['heading 150', [[2, 1.5, 1, -1, 2.5, 150, 'down']], [137.8289, -111.8395, 124.0106]], ['wrist in inner hole', [[3, 1, 1, 2.2, 0, 0, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable'], ['wrist stretched', [[2, 1, 1, 4, 0, 0, 'down']], [0.0, 0.0, 0.0]], ['heading -135', [[1.5, 1.5, 0.5, -1.2, -1.7, -135, 'down']], [-64.1702, -115.9708, 45.141]]], [['tip in reach but wrist not', [[1, 1, 1, 2.5, 0, 180, 'up']], 'unreachable'], ['wrist behind tip beyond reach', [[1, 1, 1, 1.5, 0, 180, 'up']], 'unreachable'], ['wrist stretched', [[2, 1, 1, 4, 0, 0, 'down']], [0.0, 0.0, 0.0]], ['heading -135', [[1.5, 1.5, 0.5, -1.2, -1.7, -135, 'down']], [-64.1702, -115.9708, 45.141]], ['near base with long tool', [[2, 2, 2, 0.5, 0.5, 10, 'up']], [105.7453, 136.645, 127.6097]], ['heading 300', [[2.5, 1.5, 0.8, 1, -3, 300, 'up']], [-111.095, 112.0608, -60.9658]], ['heading -300', [[2.5, 1.5, 0.8, 1, 3, -300, 'down']], [111.095, -112.0608, 60.9658]]]]
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
tool pointing along x	`[-11.5172, 80.4059, -68.8887]`	`[-11.5172, 80.4059, -68.8887]`	Passed
tool pointing up	`[73.7398, -90.0, 106.2602]`	`[73.7398, -90.0, 106.2602]`	Passed
tool pointing back	`[30.0, 120.0, 30.0]`	`[30.0, 120.0, 30.0]`	Passed
tool pointing down	`[-48.0197, 128.6822, -170.6625]`	`[-48.0197, 128.6822, -170.6625]`	Passed
wrist just reachable	`[-41.4096, 82.8192, -41.4096]`	`[-41.4096, 82.8192, -41.4096]`	Passed
tip in reach but wrist not	`unreachable`	`unreachable`	Passed
wrist behind tip beyond reach	`unreachable`	`unreachable`	Passed

SHA-256 / 8539410a79bdfc17a1f5568e03c039d9bb1182e44df01abcadebe2d4b1f89ee9

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.135211+00:00.

Case digest / 892bd7bc436edf3df1ab803caec5707e39def7fb6154fb1d185c1f4f96311a49