FA-88036 / Inverse kinematics solvers / Open access

SCARA tool roll ignores the arm joints · case 01

The tool heading is off by the sum of the shoulder and elbow angles.

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

ROOT CAUSE

t4 is set to the requested yaw directly, forgetting that t1 and t2 also rotate the tool.

VERIFIED REPAIR

Set t4 = yaw - t1 - t2.

Unsuccessful approach: Subtracting only t2 still leaves the shoulder rotation in the tool heading.

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=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)
    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]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]], ['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]], ['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]], ['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, 30.0]`	`[-11.5172, 80.4059, 4, -38.8887]`	Failed
right arm nominal	`[55.12, -80.4059, 4, 30.0]`	`[55.12, -80.4059, 4, 55.2859]`	Failed
quill fully retracted	`[16.2602, 90.0, 0, 0.0]`	`[16.2602, 90.0, 0, -106.2602]`	Failed
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 / eed2f6a8df89055a76a0e7a213ee6dfd448a88875616206ea37111961334e3ff

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
    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)-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]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]], ['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]], ['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]], ['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, -50.4059]`	`[-11.5172, 80.4059, 4, -38.8887]`	Failed
right arm nominal	`[55.12, -80.4059, 4, 110.4059]`	`[55.12, -80.4059, 4, 55.2859]`	Failed
quill fully retracted	`[16.2602, 90.0, 0, -90.0]`	`[16.2602, 90.0, 0, -106.2602]`	Failed
quill fully extended	`[90.0, -90.0, 8, 90.0]`	`[90.0, -90.0, 8, 0.0]`	Failed
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 / 8a73c9c05be0571a464a2c47a3d97d05066b6b3c247461666510444b265c98b9

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]], ['multi-turn yaw', [[4, 3, 10, 8, -2, 5, 4, 720, 'right']], [145.12, -80.4059, 6, -64.7141]], ['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]], ['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]], ['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 / 79ca6c1b216e573c09636fdb82c514a5fb0325ff4637cf776f59fbbb41a51943

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

Case digest / e31a7fc5252c594c41103913b0e68f6ee5427757dce49ab057aef1278abc49ac