FA-88291 / Inverse kinematics solvers / Open access

Articulated arm rejects fully stretched targets due to rounding · case 01

Targets exactly at full extension computed through hypot report "unreachable".

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

ROOT CAUSE

The cosine bound is tested against 1.0 with no tolerance for floating residue.

VERIFIED REPAIR

Allow a 1e-12 slack and clamp before sqrt.

Unsuccessful approach: Joining the bounds with "and" disables the reach test entirely.

Case contract

Input [d1,a2,a3,px,py,pz,reach,elbow]: base yaw t1, shoulder t2, elbow t3 with shoulder height d1. reach "front" uses t1=atan2(py,px) and r=hypot(px,py); "back" turns t1 by 180 degrees and uses -r. With s=pz-d1, cos t3=(r^2+s^2-a2^2-a3^2)/(2 a2 a3); beyond +/-(1+1e-12) is "unreachable", else clamp. elbow "up" takes sin t3<=0. t2=atan2(s,r)-atan2(a3 sin t3, a2+a3 cos t3). Angles in degrees wrapped to [-180,180), rounded 4.

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):
    d1,a2,a3,px,py,pz,reach,elbow=x
    t1=math.atan2(py,px)
    r=math.hypot(px,py)
    if reach=='back':
        t1=t1+math.pi
        r=-r
    s=pz-d1
    c3=(r*r+s*s-a2*a2-a3*a3)/(2*a2*a3)
    if c3>1.0 or c3<-1.0: return 'unreachable'
    c3=max(-1.0,min(1.0,c3))
    s3=math.sqrt(1.0-c3*c3)
    if elbow=='up': s3=-s3
    t3=math.atan2(s3,c3)
    t2=math.atan2(s,r)-math.atan2(a3*s3,a2+a3*c3)
    res=[]
    for t in (t1,t2,t3):
        d=math.degrees(t)
        d=d-360.0*math.floor((d+180.0)/360.0)
        res.append(round(d,4))
    return res
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['front nominal', [[1, 2, 2, 2, 1, 2, 'front', 'up']], [26.5651, 76.3336, -104.4775]], ['back nominal', [[1, 2, 2, 2, 1, 2, 'back', 'up']], [-153.4349, -151.8561, -104.4775]], ['front elbow down', [[1, 2, 2, 1, 2, 3, 'front', 'down']], [63.4349, 0.4007, 82.8192]], ['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['stretched diagonal', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'front', 'up']], [45.0, 0.0, -0.0]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['stretched diagonal', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'front', 'up']], [45.0, 0.0, -0.0]], ['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]], ['high tall base', [[3, 1.5, 1.5, 1, 1, 5, 'front', 'down']], [45.0, 19.4712, 70.5288]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]], ['high tall base', [[3, 1.5, 1.5, 1, 1, 5, 'front', 'down']], [45.0, 19.4712, 70.5288]], ['back third quadrant', [[1, 2, 2, -1, -2, 0, 'back', 'up']], [63.4349, -103.6664, -104.4775]], ['front negative y', [[0.5, 2, 1, 1, -1.5, 1, 'front', 'down']], [-56.3099, -14.2024, 112.0243]], ['base column below', [[4, 2, 2, 0.5, 0.5, 1, 'front', 'up']], [45.0, -37.1408, -79.1931]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['front negative y', [[0.5, 2, 1, 1, -1.5, 1, 'front', 'down']], [-56.3099, -14.2024, 112.0243]], ['base column below', [[4, 2, 2, 0.5, 0.5, 1, 'front', 'up']], [45.0, -37.1408, -79.1931]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]], ['back reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]], ['reach past base height only', [[3, 1, 1, 1, 0, 3.5, 'front', 'up']], [0.0, 82.5772, -112.0243]]], [['front nominal', [[1, 2, 2, 2, 1, 2, 'front', 'up']], [26.5651, 76.3336, -104.4775]], ['back nominal', [[1, 2, 2, 2, 1, 2, 'back', 'up']], [-153.4349, -151.8561, -104.4775]], ['front elbow down', [[1, 2, 2, 1, 2, 3, 'front', 'down']], [63.4349, 0.4007, 82.8192]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['back reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]], ['reach past base height only', [[3, 1, 1, 1, 0, 3.5, 'front', 'up']], [0.0, 82.5772, -112.0243]]]]
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
front nominal	`[26.5651, 76.3336, -104.4775]`	`[26.5651, 76.3336, -104.4775]`	Passed
back nominal	`[-153.4349, -151.8561, -104.4775]`	`[-153.4349, -151.8561, -104.4775]`	Passed
front elbow down	`[63.4349, 0.4007, 82.8192]`	`[63.4349, 0.4007, 82.8192]`	Passed
back on x axis	`[-180.0, 65.7048, 138.5904]`	`[-180.0, 65.7048, 138.5904]`	Passed
stretched diagonal	`[45.0, 0.0, -0.0]`	`[45.0, 0.0, -0.0]`	Passed
out of reach	`unreachable`	`unreachable`	Passed
stretched with rounding residue	`unreachable`	`[0.0, 8.6269, -0.0]`	Failed

SHA-256 / ccc4be5afe5799ba9fb91794dc0ffa6fb2d655892fd252af6ff1e5a0cde1c325

2 / The unsuccessful fix

Exit 1

"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
    d1,a2,a3,px,py,pz,reach,elbow=x
    t1=math.atan2(py,px)
    r=math.hypot(px,py)
    if reach=='back':
        t1=t1+math.pi
        r=-r
    s=pz-d1
    c3=(r*r+s*s-a2*a2-a3*a3)/(2*a2*a3)
    if c3>1.0+1e-12 and c3<-1.0-1e-12: return 'unreachable'
    c3=max(-1.0,min(1.0,c3))
    s3=math.sqrt(1.0-c3*c3)
    if elbow=='up': s3=-s3
    t3=math.atan2(s3,c3)
    t2=math.atan2(s,r)-math.atan2(a3*s3,a2+a3*c3)
    res=[]
    for t in (t1,t2,t3):
        d=math.degrees(t)
        d=d-360.0*math.floor((d+180.0)/360.0)
        res.append(round(d,4))
    return res
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['front nominal', [[1, 2, 2, 2, 1, 2, 'front', 'up']], [26.5651, 76.3336, -104.4775]], ['back nominal', [[1, 2, 2, 2, 1, 2, 'back', 'up']], [-153.4349, -151.8561, -104.4775]], ['front elbow down', [[1, 2, 2, 1, 2, 3, 'front', 'down']], [63.4349, 0.4007, 82.8192]], ['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['stretched diagonal', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'front', 'up']], [45.0, 0.0, -0.0]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['stretched diagonal', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'front', 'up']], [45.0, 0.0, -0.0]], ['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]], ['high tall base', [[3, 1.5, 1.5, 1, 1, 5, 'front', 'down']], [45.0, 19.4712, 70.5288]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]], ['high tall base', [[3, 1.5, 1.5, 1, 1, 5, 'front', 'down']], [45.0, 19.4712, 70.5288]], ['back third quadrant', [[1, 2, 2, -1, -2, 0, 'back', 'up']], [63.4349, -103.6664, -104.4775]], ['front negative y', [[0.5, 2, 1, 1, -1.5, 1, 'front', 'down']], [-56.3099, -14.2024, 112.0243]], ['base column below', [[4, 2, 2, 0.5, 0.5, 1, 'front', 'up']], [45.0, -37.1408, -79.1931]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['front negative y', [[0.5, 2, 1, 1, -1.5, 1, 'front', 'down']], [-56.3099, -14.2024, 112.0243]], ['base column below', [[4, 2, 2, 0.5, 0.5, 1, 'front', 'up']], [45.0, -37.1408, -79.1931]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]], ['back reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]], ['reach past base height only', [[3, 1, 1, 1, 0, 3.5, 'front', 'up']], [0.0, 82.5772, -112.0243]]], [['front nominal', [[1, 2, 2, 2, 1, 2, 'front', 'up']], [26.5651, 76.3336, -104.4775]], ['back nominal', [[1, 2, 2, 2, 1, 2, 'back', 'up']], [-153.4349, -151.8561, -104.4775]], ['front elbow down', [[1, 2, 2, 1, 2, 3, 'front', 'down']], [63.4349, 0.4007, 82.8192]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['back reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]], ['reach past base height only', [[3, 1, 1, 1, 0, 3.5, 'front', 'up']], [0.0, 82.5772, -112.0243]]]]
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
front nominal	`[26.5651, 76.3336, -104.4775]`	`[26.5651, 76.3336, -104.4775]`	Passed
back nominal	`[-153.4349, -151.8561, -104.4775]`	`[-153.4349, -151.8561, -104.4775]`	Passed
front elbow down	`[63.4349, 0.4007, 82.8192]`	`[63.4349, 0.4007, 82.8192]`	Passed
back on x axis	`[-180.0, 65.7048, 138.5904]`	`[-180.0, 65.7048, 138.5904]`	Passed
stretched diagonal	`[45.0, 0.0, -0.0]`	`[45.0, 0.0, -0.0]`	Passed
out of reach	`[45.0, 0.0, -0.0]`	`unreachable`	Failed
stretched with rounding residue	`[0.0, 8.6269, -0.0]`	`[0.0, 8.6269, -0.0]`	Passed

SHA-256 / a936437d9b56c8ea14ab65ea9d23c0ceaa4919720d2589f2d6ebb8da5971abb8

3 / The verified repair

Exit 0

"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
    d1,a2,a3,px,py,pz,reach,elbow=x
    t1=math.atan2(py,px)
    r=math.hypot(px,py)
    if reach=='back':
        t1=t1+math.pi
        r=-r
    s=pz-d1
    c3=(r*r+s*s-a2*a2-a3*a3)/(2*a2*a3)
    if c3>1.0+1e-12 or c3<-1.0-1e-12: return 'unreachable'
    c3=max(-1.0,min(1.0,c3))
    s3=math.sqrt(1.0-c3*c3)
    if elbow=='up': s3=-s3
    t3=math.atan2(s3,c3)
    t2=math.atan2(s,r)-math.atan2(a3*s3,a2+a3*c3)
    res=[]
    for t in (t1,t2,t3):
        d=math.degrees(t)
        d=d-360.0*math.floor((d+180.0)/360.0)
        res.append(round(d,4))
    return res
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['front nominal', [[1, 2, 2, 2, 1, 2, 'front', 'up']], [26.5651, 76.3336, -104.4775]], ['back nominal', [[1, 2, 2, 2, 1, 2, 'back', 'up']], [-153.4349, -151.8561, -104.4775]], ['front elbow down', [[1, 2, 2, 1, 2, 3, 'front', 'down']], [63.4349, 0.4007, 82.8192]], ['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['stretched diagonal', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'front', 'up']], [45.0, 0.0, -0.0]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['stretched diagonal', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'front', 'up']], [45.0, 0.0, -0.0]], ['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]], ['high tall base', [[3, 1.5, 1.5, 1, 1, 5, 'front', 'down']], [45.0, 19.4712, 70.5288]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]], ['high tall base', [[3, 1.5, 1.5, 1, 1, 5, 'front', 'down']], [45.0, 19.4712, 70.5288]], ['back third quadrant', [[1, 2, 2, -1, -2, 0, 'back', 'up']], [63.4349, -103.6664, -104.4775]], ['front negative y', [[0.5, 2, 1, 1, -1.5, 1, 'front', 'down']], [-56.3099, -14.2024, 112.0243]], ['base column below', [[4, 2, 2, 0.5, 0.5, 1, 'front', 'up']], [45.0, -37.1408, -79.1931]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]]], [['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['front negative y', [[0.5, 2, 1, 1, -1.5, 1, 'front', 'down']], [-56.3099, -14.2024, 112.0243]], ['base column below', [[4, 2, 2, 0.5, 0.5, 1, 'front', 'up']], [45.0, -37.1408, -79.1931]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]], ['back reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]], ['reach past base height only', [[3, 1, 1, 1, 0, 3.5, 'front', 'up']], [0.0, 82.5772, -112.0243]]], [['front nominal', [[1, 2, 2, 2, 1, 2, 'front', 'up']], [26.5651, 76.3336, -104.4775]], ['back nominal', [[1, 2, 2, 2, 1, 2, 'back', 'up']], [-153.4349, -151.8561, -104.4775]], ['front elbow down', [[1, 2, 2, 1, 2, 3, 'front', 'down']], [63.4349, 0.4007, 82.8192]], ['out of reach', [[1, 1, 1, 2, 2, 1, 'front', 'up']], 'unreachable'], ['back reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]], ['stretched with rounding residue', [[0, 1, 1, 1.977371993328519, 0, 0.3, 'front', 'up']], [0.0, 8.6269, -0.0]], ['reach past base height only', [[3, 1, 1, 1, 0, 3.5, 'front', 'up']], [0.0, 82.5772, -112.0243]]]]
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
front nominal	`[26.5651, 76.3336, -104.4775]`	`[26.5651, 76.3336, -104.4775]`	Passed
back nominal	`[-153.4349, -151.8561, -104.4775]`	`[-153.4349, -151.8561, -104.4775]`	Passed
front elbow down	`[63.4349, 0.4007, 82.8192]`	`[63.4349, 0.4007, 82.8192]`	Passed
back on x axis	`[-180.0, 65.7048, 138.5904]`	`[-180.0, 65.7048, 138.5904]`	Passed
stretched diagonal	`[45.0, 0.0, -0.0]`	`[45.0, 0.0, -0.0]`	Passed
out of reach	`unreachable`	`unreachable`	Passed
stretched with rounding residue	`[0.0, 8.6269, -0.0]`	`[0.0, 8.6269, -0.0]`	Passed

SHA-256 / 26a28470b32c260e4b665b2ba303bfcc16f4d1a3b092c5b598bb271e1c39c2e9

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

Case digest / 5292940e2be0b7d10f994ed9e51121cc79e1889f3aaf8618e158d191bfcb5a10