FA-87886 / Inverse kinematics solvers / Open access

Wrist joint ignores the shoulder angle when matching heading · case 01

The tool heading is off by exactly the shoulder angle.

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

ROOT CAUSE

q3 is set to phi-q2, forgetting that the heading is the sum of all three joints.

VERIFIED REPAIR

Set q3 = phi - q1 - q2.

Unsuccessful approach: Writing q1+q2-phi reverses the sign of the wrist command.

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 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-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'], ['diagonal heading', [[2, 1, 1, 2, 2, 45, 'down']], [74.8541, -114.4698, 84.6157]]], [['tool pointing up', [[2, 1.5, 0.5, 2, 2, 90, 'down']], [73.7398, -90.0, 106.2602]], ['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]], ['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]]], [['tool pointing back', [[2, 2, 1, -1, 2, 180, 'up']], [30.0, 120.0, 30.0]], ['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 in inner hole', [[3, 1, 1, 2.2, 0, 0, 'up']], 'unreachable']], [['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['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]]], [['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['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, -80.4059]`	`[-11.5172, 80.4059, -68.8887]`	Failed
tool pointing up	`[73.7398, -90.0, -180.0]`	`[73.7398, -90.0, 106.2602]`	Failed
tool pointing back	`[30.0, 120.0, 60.0]`	`[30.0, 120.0, 30.0]`	Failed
tool pointing down	`[-48.0197, 128.6822, 141.3178]`	`[-48.0197, 128.6822, -170.6625]`	Failed
wrist just reachable	`[-41.4096, 82.8192, -82.8192]`	`[-41.4096, 82.8192, -41.4096]`	Failed
tip in reach but wrist not	`unreachable`	`unreachable`	Passed
diagonal heading	`[74.8541, -114.4698, 159.4698]`	`[74.8541, -114.4698, 84.6157]`	Failed

SHA-256 / 51f672a453eaea14deee9913e29dfc89be938ab34fab4b69ce0930e9995a045a

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)**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=q1+q2-a
    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'], ['diagonal heading', [[2, 1, 1, 2, 2, 45, 'down']], [74.8541, -114.4698, 84.6157]]], [['tool pointing up', [[2, 1.5, 0.5, 2, 2, 90, 'down']], [73.7398, -90.0, 106.2602]], ['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]], ['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]]], [['tool pointing back', [[2, 2, 1, -1, 2, 180, 'up']], [30.0, 120.0, 30.0]], ['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 in inner hole', [[3, 1, 1, 2.2, 0, 0, 'up']], 'unreachable']], [['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['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]]], [['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['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]`	Failed
tool pointing up	`[73.7398, -90.0, -106.2602]`	`[73.7398, -90.0, 106.2602]`	Failed
tool pointing back	`[30.0, 120.0, -30.0]`	`[30.0, 120.0, 30.0]`	Failed
tool pointing down	`[-48.0197, 128.6822, 170.6625]`	`[-48.0197, 128.6822, -170.6625]`	Failed
wrist just reachable	`[-41.4096, 82.8192, 41.4096]`	`[-41.4096, 82.8192, -41.4096]`	Failed
tip in reach but wrist not	`unreachable`	`unreachable`	Passed
diagonal heading	`[74.8541, -114.4698, -84.6157]`	`[74.8541, -114.4698, 84.6157]`	Failed

SHA-256 / 21e7e18eda4dc8e19bbc906b42c9cbe64e343928ee32bbb46fb2232e38e949da

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'], ['diagonal heading', [[2, 1, 1, 2, 2, 45, 'down']], [74.8541, -114.4698, 84.6157]]], [['tool pointing up', [[2, 1.5, 0.5, 2, 2, 90, 'down']], [73.7398, -90.0, 106.2602]], ['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]], ['third quadrant wrist', [[2, 2, 0.5, -2, -2, 225, 'up']], [170.5989, 108.8023, -54.4011]]], [['tool pointing back', [[2, 2, 1, -1, 2, 180, 'up']], [30.0, 120.0, 30.0]], ['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 in inner hole', [[3, 1, 1, 2.2, 0, 0, 'up']], 'unreachable']], [['tool pointing down', [[2, 1, 0.5, 1.5, -1, -90, 'up']], [-48.0197, 128.6822, -170.6625]], ['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]]], [['wrist just reachable', [[1, 1, 1, 2.5, 0, 0, 'up']], [-41.4096, 82.8192, -41.4096]], ['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
diagonal heading	`[74.8541, -114.4698, 84.6157]`	`[74.8541, -114.4698, 84.6157]`	Passed

SHA-256 / c43b1e66aa28561ede91e75e13b41df86a7b2873aa0c3f8228cfda2578f68b85

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

Case digest / 3d46e4d05d38a8552b49ae8b33f44dae82929e2bef7c3e41243a89ba79ef65ec