FA-88026 / Inverse kinematics solvers / Open access

SCARA quill extension is computed upward · case 01

Reachable heights are rejected and heights above the home plane are accepted.

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

ROOT CAUSE

d3 is computed as pz-d0 although the prismatic joint extends downward from d0.

VERIFIED REPAIR

Use d3 = d0 - pz.

Unsuccessful approach: Taking the absolute value accepts targets above the home plane as if they were below it.

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=pz-d0
    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 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'], ['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]]], [['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['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]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['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 fixture	Actual	Expected	Outcome
left arm nominal	`out-of-stroke`	`[-11.5172, 80.4059, 4, -38.8887]`	Failed
right arm nominal	`out-of-stroke`	`[55.12, -80.4059, 4, 55.2859]`	Failed
quill fully retracted	`[16.2602, 90.0, 0, -106.2602]`	`[16.2602, 90.0, 0, -106.2602]`	Passed
quill fully extended	`out-of-stroke`	`[90.0, -90.0, 8, 0.0]`	Failed
target above quill home	`[16.2602, 90.0, 2, -106.2602]`	`out-of-stroke`	Failed
below stroke	`out-of-stroke`	`out-of-stroke`	Passed
out of stroke and reach	`unreachable`	`out-of-stroke`	Failed

SHA-256 / 2a17b092be7b8c0176a85c0e4159e752e917dea039f746b6eb12156006f8ed0a

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=abs(pz-d0)
    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 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'], ['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]]], [['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['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]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['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 fixture	Actual	Expected	Outcome
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 home	`[16.2602, 90.0, 2, -106.2602]`	`out-of-stroke`	Failed
below stroke	`out-of-stroke`	`out-of-stroke`	Passed
out of stroke and reach	`unreachable`	`out-of-stroke`	Failed

SHA-256 / 61b6fa0bcc0d7369849de2ea65a0c3b55918f5f2bee78a7318eed5f7ce95ee98

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 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'], ['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]]], [['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['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]], ['target above quill home', [[4, 3, 10, 8, 3, 4, 12, 0, 'left']], 'out-of-stroke'], ['out of stroke and reach', [[4, 3, 10, 8, 9, 0, 12, 0, 'left']], 'out-of-stroke'], ['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 fixture	Actual	Expected	Outcome
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 home	`out-of-stroke`	`out-of-stroke`	Passed
below stroke	`out-of-stroke`	`out-of-stroke`	Passed
out of stroke and reach	`out-of-stroke`	`out-of-stroke`	Passed

SHA-256 / 26aab6c7961205021ef46a1862ebfd6b79e4cbd1738dbef388a367b3806358b2

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

Case digest / 1aa5af4e2216fa4425f1446bca7b6ee2515ab53c51f2f57772953acc58fe3cb3