FA-88286 / Inverse kinematics solvers / Open access

Joint wrap rounds to the nearest turn and returns +180 · case 01

A base yaw of exactly 180 is reported as +180 instead of -180.

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

ROOT CAUSE

The wrap uses round(d/360), whose half-even rounding leaves +180 in the output.

VERIFIED REPAIR

Use d - 360*floor((d+180)/360) to land in [-180,180).

Unsuccessful approach: math.fmod keeps the sign of the dividend and mis-wraps negative angles.

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,r)-math.atan2(a3*s3,a2+a3*c3)
    res=[]
    for t in (t1,t2,t3):
        d=math.degrees(t)
        d=d-360.0*round(d/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]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]]], [['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 reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]]], [['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['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]]], [['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['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 on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]], ['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]`	Failed
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]`	Failed
back reach below shoulder	`[67.6199, 165.5945, 107.2325]`	`[67.6199, 165.5945, 107.2325]`	Passed

SHA-256 / 21fe1cff09bc1d1f87a6936c9f2de37582294a61837a5bd37309f191fffb5d02

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(s,r)-math.atan2(a3*s3,a2+a3*c3)
    res=[]
    for t in (t1,t2,t3):
        d=math.degrees(t)
        d=math.fmod(d+180.0,360.0)-180.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]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]]], [['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 reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]]], [['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['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]]], [['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['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 on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]], ['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
back reach below shoulder	`[67.6199, -194.4055, 107.2325]`	`[67.6199, 165.5945, 107.2325]`	Failed

SHA-256 / 3870a4026258b8c34adec2a33b7a372dd21cb0ed7d021f580a87b244373da6ae

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]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]]], [['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 reach low front quadrant', [[1.4, 2, 2, 1.5, -0.6, 1.0, 'back', 'down']], [158.1986, 128.4939, 130.8249]]], [['back on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['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]]], [['stretched diagonal back', [[0, 1, 1, 1.4142135623730951, 1.4142135623730951, 0, 'back', 'down']], [-135.0, -180.0, 0.0]], ['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 on x axis', [[1, 2, 2, 1, 0, 2, 'back', 'down']], [-180.0, 65.7048, 138.5904]], ['back reach below shoulder', [[1.4, 2, 2, -0.7, -1.7, -0.1, 'back', 'down']], [67.6199, 165.5945, 107.2325]], ['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
back reach below shoulder	`[67.6199, 165.5945, 107.2325]`	`[67.6199, 165.5945, 107.2325]`	Passed

SHA-256 / 1c80c8754ce309ebaf87d81baea7720cb7fab6de63f41df734d28f31313beae0

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

Case digest / 841f2cb0c06d74dea4ac410491fea81f56cd330fdc4b8e2a242dc61dee0b842f