FAILURE MAP
← Case archive

FA-88046 / Inverse kinematics solvers / Open access

SCARA law of cosines receives the radius instead of its square · case 01

Elbow angles are wrong everywhere except at unit radius.

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

ROOT CAUSE

r2 is computed with hypot, so the cosine formula mixes a length with squared lengths.

VERIFIED REPAIR

Use px^2+py^2.

Unsuccessful approach: Including pz^2 in the radius treats the vertical axis as part of the planar reach.

Case contract

Input [l1,l2,d0,stroke,px,py,pz,yaw,arm]. The quill extends downward: d3=d0-pz must lie in [0,stroke] else "out-of-stroke" (checked first). Planar reach uses cos t2=(px^2+py^2-l1^2-l2^2)/(2 l1 l2); |cos|>1 returns "unreachable". arm "right" takes t2<=0. Tool yaw (degrees) equals t1+t2+t4. Return [t1,t2,d3,t4] with angles in degrees wrapped to (-180,180], all rounded to 4 places.

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,d0,stroke,px,py,pz,yaw,arm=x
    d3=d0-pz
    if d3<0 or d3>stroke: return 'out-of-stroke'
    r2=math.hypot(px,py)
    c=(r2-l1*l1-l2*l2)/(2.0*l1*l2)
    if c>1.0 or c<-1.0: return 'unreachable'
    s=math.sqrt(1.0-c*c)
    if arm=='right': s=-s
    t2=math.atan2(s,c)
    t1=math.atan2(py,px)-math.atan2(l2*s,l1+l2*c)
    t4=math.radians(yaw)-t1-t2
    res=[]
    for t in (t1,t2,t4):
        d=math.degrees(t)
        while d>180.0: d-=360.0
        while d<=-180.0: d+=360.0
        res.append(round(d,4))
    return [res[0],res[1],round(d3,4),res[2]]
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['left arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'left']], [-11.5172, 80.4059, 4, -38.8887]], ['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['quill fully extended', [[4, 3, 10, 8, 3, 4, 2, 0, 'right']], [90.0, -90.0, 8, 0.0]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['below stroke', [[4, 3, 10, 8, 3, 4, 1, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke']], [['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['below stroke', [[4, 3, 10, 8, 3, 4, 1, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]]], [['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]], ['negative yaw', [[4, 3, 10, 8, -4, -3, 4, -170, 'left']], [180.0, 90.0, 6, -80.0]], ['yaw 180 seam', [[4, 3, 10, 8, 0, 6, 7, 180, 'right']], [116.3843, -62.7204, 3, 126.3361]], ['reach between r and r2', [[1, 1, 10, 8, 1.2, 1.1, 5, 0, 'left']], [6.9932, 71.0344, 5, -78.0277]]], [['quill fully extended', [[4, 3, 10, 8, 3, 4, 2, 0, 'right']], [90.0, -90.0, 8, 0.0]], ['negative yaw', [[4, 3, 10, 8, -4, -3, 4, -170, 'left']], [180.0, 90.0, 6, -80.0]], ['yaw 180 seam', [[4, 3, 10, 8, 0, 6, 7, 180, 'right']], [116.3843, -62.7204, 3, 126.3361]], ['reach between r and r2', [[1, 1, 10, 8, 1.2, 1.1, 5, 0, 'left']], [6.9932, 71.0344, 5, -78.0277]], ['back quadrant', [[5, 4, 10, 8, -6, -2, 3, 250, 'right']], [-122.3482, -91.4325, 7, 103.7807]], ['near base hole', [[4, 3, 10, 8, 0.5, 0.2, 5, 0, 'left']], 'unreachable'], ['yaw -540', [[4, 3, 10, 8, 2, -5, 9, -540, 'left']], [-101.5172, 80.4059, 1, -158.8887]]], [['left arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'left']], [-11.5172, 80.4059, 4, -38.8887]], ['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['near base hole', [[4, 3, 10, 8, 0.5, 0.2, 5, 0, 'left']], 'unreachable'], ['yaw -540', [[4, 3, 10, 8, 2, -5, 9, -540, 'left']], [-101.5172, 80.4059, 1, -158.8887]]]]
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 fixtureActualExpectedOutcome
left arm nominal[-26.3524, 144.8139, 4, -88.4615][-11.5172, 80.4059, 4, -38.8887]Failed
right arm nominal[69.9553, -144.8139, 4, 104.8587][55.12, -80.4059, 4, 55.2859]Failed
quill fully retracted[5.2605, 146.4427, 0, -151.7032][16.2602, 90.0, 0, -106.2602]Failed
quill fully extended[100.9997, -146.4427, 8, 45.443][90.0, -90.0, 8, 0.0]Failed
target above quill homeout-of-strokeout-of-strokePassed
below strokeout-of-strokeout-of-strokePassed
out of stroke and reachout-of-strokeout-of-strokePassed

SHA-256 / 4ad3598d698035e0aaa1d6bc5d74c658d03bd713c5ea9cccddfc3ba63f4aa76d

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,d0,stroke,px,py,pz,yaw,arm=x
    d3=d0-pz
    if d3<0 or d3>stroke: return 'out-of-stroke'
    r2=px*px+py*py+pz*pz
    c=(r2-l1*l1-l2*l2)/(2.0*l1*l2)
    if c>1.0 or c<-1.0: return 'unreachable'
    s=math.sqrt(1.0-c*c)
    if arm=='right': s=-s
    t2=math.atan2(s,c)
    t1=math.atan2(py,px)-math.atan2(l2*s,l1+l2*c)
    t4=math.radians(yaw)-t1-t2
    res=[]
    for t in (t1,t2,t4):
        d=math.degrees(t)
        while d>180.0: d-=360.0
        while d<=-180.0: d+=360.0
        res.append(round(d,4))
    return [res[0],res[1],round(d3,4),res[2]]
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['left arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'left']], [-11.5172, 80.4059, 4, -38.8887]], ['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['quill fully extended', [[4, 3, 10, 8, 3, 4, 2, 0, 'right']], [90.0, -90.0, 8, 0.0]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['below stroke', [[4, 3, 10, 8, 3, 4, 1, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke']], [['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['below stroke', [[4, 3, 10, 8, 3, 4, 1, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]]], [['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]], ['negative yaw', [[4, 3, 10, 8, -4, -3, 4, -170, 'left']], [180.0, 90.0, 6, -80.0]], ['yaw 180 seam', [[4, 3, 10, 8, 0, 6, 7, 180, 'right']], [116.3843, -62.7204, 3, 126.3361]], ['reach between r and r2', [[1, 1, 10, 8, 1.2, 1.1, 5, 0, 'left']], [6.9932, 71.0344, 5, -78.0277]]], [['quill fully extended', [[4, 3, 10, 8, 3, 4, 2, 0, 'right']], [90.0, -90.0, 8, 0.0]], ['negative yaw', [[4, 3, 10, 8, -4, -3, 4, -170, 'left']], [180.0, 90.0, 6, -80.0]], ['yaw 180 seam', [[4, 3, 10, 8, 0, 6, 7, 180, 'right']], [116.3843, -62.7204, 3, 126.3361]], ['reach between r and r2', [[1, 1, 10, 8, 1.2, 1.1, 5, 0, 'left']], [6.9932, 71.0344, 5, -78.0277]], ['back quadrant', [[5, 4, 10, 8, -6, -2, 3, 250, 'right']], [-122.3482, -91.4325, 7, 103.7807]], ['near base hole', [[4, 3, 10, 8, 0.5, 0.2, 5, 0, 'left']], 'unreachable'], ['yaw -540', [[4, 3, 10, 8, 2, -5, 9, -540, 'left']], [-101.5172, 80.4059, 1, -158.8887]]], [['left arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'left']], [-11.5172, 80.4059, 4, -38.8887]], ['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['near base hole', [[4, 3, 10, 8, 0.5, 0.2, 5, 0, 'left']], 'unreachable'], ['yaw -540', [[4, 3, 10, 8, 2, -5, 9, -540, 'left']], [-101.5172, 80.4059, 1, -158.8887]]]]
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 fixtureActualExpectedOutcome
left arm nominalunreachable[-11.5172, 80.4059, 4, -38.8887]Failed
right arm nominalunreachable[55.12, -80.4059, 4, 55.2859]Failed
quill fully retractedunreachable[16.2602, 90.0, 0, -106.2602]Failed
quill fully extended[86.4487, -80.4059, 8, -6.0428][90.0, -90.0, 8, 0.0]Failed
target above quill homeout-of-strokeout-of-strokePassed
below strokeout-of-strokeout-of-strokePassed
out of stroke and reachout-of-strokeout-of-strokePassed

SHA-256 / 1b318910ec5b80942d4d6a7d00e97dcf2da592d94b37cf6d2d1f1c2b9a55bb63

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,d0,stroke,px,py,pz,yaw,arm=x
    d3=d0-pz
    if d3<0 or d3>stroke: return 'out-of-stroke'
    r2=px*px+py*py
    c=(r2-l1*l1-l2*l2)/(2.0*l1*l2)
    if c>1.0 or c<-1.0: return 'unreachable'
    s=math.sqrt(1.0-c*c)
    if arm=='right': s=-s
    t2=math.atan2(s,c)
    t1=math.atan2(py,px)-math.atan2(l2*s,l1+l2*c)
    t4=math.radians(yaw)-t1-t2
    res=[]
    for t in (t1,t2,t4):
        d=math.degrees(t)
        while d>180.0: d-=360.0
        while d<=-180.0: d+=360.0
        res.append(round(d,4))
    return [res[0],res[1],round(d3,4),res[2]]
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['left arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'left']], [-11.5172, 80.4059, 4, -38.8887]], ['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['quill fully extended', [[4, 3, 10, 8, 3, 4, 2, 0, 'right']], [90.0, -90.0, 8, 0.0]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['below stroke', [[4, 3, 10, 8, 3, 4, 1, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke']], [['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['below stroke', [[4, 3, 10, 8, 3, 4, 1, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]]], [['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]], ['negative yaw', [[4, 3, 10, 8, -4, -3, 4, -170, 'left']], [180.0, 90.0, 6, -80.0]], ['yaw 180 seam', [[4, 3, 10, 8, 0, 6, 7, 180, 'right']], [116.3843, -62.7204, 3, 126.3361]], ['reach between r and r2', [[1, 1, 10, 8, 1.2, 1.1, 5, 0, 'left']], [6.9932, 71.0344, 5, -78.0277]]], [['quill fully extended', [[4, 3, 10, 8, 3, 4, 2, 0, 'right']], [90.0, -90.0, 8, 0.0]], ['negative yaw', [[4, 3, 10, 8, -4, -3, 4, -170, 'left']], [180.0, 90.0, 6, -80.0]], ['yaw 180 seam', [[4, 3, 10, 8, 0, 6, 7, 180, 'right']], [116.3843, -62.7204, 3, 126.3361]], ['reach between r and r2', [[1, 1, 10, 8, 1.2, 1.1, 5, 0, 'left']], [6.9932, 71.0344, 5, -78.0277]], ['back quadrant', [[5, 4, 10, 8, -6, -2, 3, 250, 'right']], [-122.3482, -91.4325, 7, 103.7807]], ['near base hole', [[4, 3, 10, 8, 0.5, 0.2, 5, 0, 'left']], 'unreachable'], ['yaw -540', [[4, 3, 10, 8, 2, -5, 9, -540, 'left']], [-101.5172, 80.4059, 1, -158.8887]]], [['left arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'left']], [-11.5172, 80.4059, 4, -38.8887]], ['right arm nominal', [[4, 3, 10, 8, 5, 2, 6, 30, 'right']], [55.12, -80.4059, 4, 55.2859]], ['quill fully retracted', [[4, 3, 10, 8, 3, 4, 10, 0, 'left']], [16.2602, 90.0, 0, -106.2602]], ['out of reach only', [[4, 3, 10, 8, 9, 0, 5, 0, 'left']], 'unreachable'], ['fully stretched', [[4, 3, 10, 8, 7, 0, 5, 45, 'left']], [0.0, 0.0, 5, 45.0]], ['near base hole', [[4, 3, 10, 8, 0.5, 0.2, 5, 0, 'left']], 'unreachable'], ['yaw -540', [[4, 3, 10, 8, 2, -5, 9, -540, 'left']], [-101.5172, 80.4059, 1, -158.8887]]]]
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 fixtureActualExpectedOutcome
left arm nominal[-11.5172, 80.4059, 4, -38.8887][-11.5172, 80.4059, 4, -38.8887]Passed
right arm nominal[55.12, -80.4059, 4, 55.2859][55.12, -80.4059, 4, 55.2859]Passed
quill fully retracted[16.2602, 90.0, 0, -106.2602][16.2602, 90.0, 0, -106.2602]Passed
quill fully extended[90.0, -90.0, 8, 0.0][90.0, -90.0, 8, 0.0]Passed
target above quill homeout-of-strokeout-of-strokePassed
below strokeout-of-strokeout-of-strokePassed
out of stroke and reachout-of-strokeout-of-strokePassed

SHA-256 / 2ab04c3c11eadb0ec814b1290d5e7cb121f4a33a2aa24ebfa284fee99f4e780f

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

Case digest / c205f61945970123f78a158ecfbadcd215b723a20ee19af1c5e63afb723bbec2