FA-88281 / Inverse kinematics solvers / Open access

Back-reach shoulder elevation uses the unsigned radius · case 01

Over-the-back solutions come out mirrored in front of the robot.

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

ROOT CAUSE

The shoulder elevation is atan2(s,|r|), discarding the negated radius.

VERIFIED REPAIR

Use atan2(s, r) with the signed radius.

Unsuccessful approach: Taking |s| instead mirrors targets below the shoulder.

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+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,abs(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]], ['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]]], [['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]], ['back third quadrant', [[1, 2, 2, -1, -2, 0, 'back', 'up']], [63.4349, -103.6664, -104.4775]]], [['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['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]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]]], [['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]], ['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]], ['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]]]]
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, 76.3336, -104.4775]`	`[-153.4349, -151.8561, -104.4775]`	Failed
front elbow down	`[63.4349, 0.4007, 82.8192]`	`[63.4349, 0.4007, 82.8192]`	Passed
back on x axis	`[-180.0, -24.2952, 138.5904]`	`[-180.0, 65.7048, 138.5904]`	Failed
stretched diagonal	`[45.0, 0.0, -0.0]`	`[45.0, 0.0, -0.0]`	Passed
stretched diagonal back	`[-135.0, 0.0, 0.0]`	`[-135.0, -180.0, 0.0]`	Failed
below base	`[-33.6901, -0.3729, -97.1808]`	`[-33.6901, -0.3729, -97.1808]`	Passed

SHA-256 / 01c6c0fbb5813c3beeeb1cf7550314d20d8d886f1d1efbdc7306cc536836f21f

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 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(abs(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]], ['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]]], [['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]], ['back third quadrant', [[1, 2, 2, -1, -2, 0, 'back', 'up']], [63.4349, -103.6664, -104.4775]]], [['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['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]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]]], [['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]], ['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]], ['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]]]]
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
stretched diagonal back	`[-135.0, -180.0, 0.0]`	`[-135.0, -180.0, 0.0]`	Passed
below base	`[-33.6901, 79.1514, -97.1808]`	`[-33.6901, -0.3729, -97.1808]`	Failed

SHA-256 / af7c2b9406bea690e089885ace4e6d97c2a3d7d95a952945368b291f42853fac

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]], ['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['below base', [[2, 2, 1.5, 1.5, -1, 0.5, 'front', 'up']], [-33.6901, -0.3729, -97.1808]]], [['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]], ['back third quadrant', [[1, 2, 2, -1, -2, 0, 'back', 'up']], [63.4349, -103.6664, -104.4775]]], [['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['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]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]]], [['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]], ['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]], ['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]]]]
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
stretched diagonal back	`[-135.0, -180.0, 0.0]`	`[-135.0, -180.0, 0.0]`	Passed
below base	`[-33.6901, -0.3729, -97.1808]`	`[-33.6901, -0.3729, -97.1808]`	Passed

SHA-256 / 6a2944707159616fdd6ed4f9b3e9985320baa0cf2e01790c9152adc93aa77dc7

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

Case digest / 54a69e6da09b9f54c88071d21f3defeed143964a1223b10286f19c70c2bf1193