FA-88116 / Inverse kinematics solvers / Open access

Stewart platform composes roll, pitch and yaw in reverse order · case 01

Poses with more than one non-zero attitude angle produce wrong leg lengths.

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

ROOT CAUSE

The rotation is built as Rx Ry Rz instead of the contract Rz Ry Rx.

VERIFIED REPAIR

Compose R = Rz(yaw) Ry(pitch) Rx(roll).

Unsuccessful approach: Applying roll after yaw but before pitch still uses a different Euler sequence.

Case contract

Input [A,B,pose,lmin,lmax]: base anchors A and platform anchors B (platform frame), pose [x,y,z,roll,pitch,yaw] in degrees with R = Rz(yaw) Ry(pitch) Rx(roll). Leg i length is |p + R b_i - a_i|. Return lengths rounded to 4, or ["limit", i, length] for the first leg outside the inclusive range [lmin,lmax].

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):
    A,B,pose,lmin,lmax=x
    px,py,pz,ro,pi_,ya=pose
    cr,sr=math.cos(math.radians(ro)),math.sin(math.radians(ro))
    cp,sp=math.cos(math.radians(pi_)),math.sin(math.radians(pi_))
    cy,sy=math.cos(math.radians(ya)),math.sin(math.radians(ya))
    Rx=[[1,0,0],[0,cr,-sr],[0,sr,cr]]
    Ry=[[cp,0,sp],[0,1,0],[-sp,0,cp]]
    Rz=[[cy,-sy,0],[sy,cy,0],[0,0,1]]
    mm=lambda P,Q:[[sum(P[i][k]*Q[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
    Rm=mm(Rx,mm(Ry,Rz))
    out=[]
    for i,(a,b) in enumerate(zip(A,B)):
        w=[sum(Rm[r][c]*b[c] for c in range(3)) for r in range(3)]
        v=[px+w[0]-a[0],py+w[1]-a[1],pz+w[2]-a[2]]
        L=math.sqrt(v[0]**2+v[1]**2+v[2]**2)
        if L<lmin or L>lmax: return ['limit',i,round(L,4)]
        out.append(round(L,4))
    return out
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['upper bound exact', [[[[0, 0, 0], [1, 0, 0]], [[3, 4, 0], [4, 4, 0]], [0, 0, 12, 0, 0, 0], 5, 13]], [13.0, 13.0]], ['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]]], [['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]], ['long leg second', [[[[0, 0, 0], [8, 0, 0]], [[0, 0, 0], [0, 0, 0]], [0, 0, 6, 0, 0, 0], 1, 9]], ['limit', 1, 10.0]]], [['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]], ['long leg second', [[[[0, 0, 0], [8, 0, 0]], [[0, 0, 0], [0, 0, 0]], [0, 0, 6, 0, 0, 0], 1, 9]], ['limit', 1, 10.0]], ['yaw with offsets', [[[[3, 0, 0], [-3, 0, 0]], [[1, 0, 0], [-1, 0, 0]], [1, 2, 7, 5, 5, 60], 1, 30]], [7.6314, 7.9865]], ['large pitch', [[[[2, 0, 0], [0, 2, 0], [-2, 0, 0]], [[1, 0, 0], [0, 1, 0], [-1, 0, 0]], [0, 0, 5, 0, 60, 10], 1, 30]], [4.4012, 5.105, 6.0573]]], [['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['upper bound exact', [[[[0, 0, 0], [1, 0, 0]], [[3, 4, 0], [4, 4, 0]], [0, 0, 12, 0, 0, 0], 5, 13]], [13.0, 13.0]], ['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['yaw with offsets', [[[[3, 0, 0], [-3, 0, 0]], [[1, 0, 0], [-1, 0, 0]], [1, 2, 7, 5, 5, 60], 1, 30]], [7.6314, 7.9865]], ['large pitch', [[[[2, 0, 0], [0, 2, 0], [-2, 0, 0]], [[1, 0, 0], [0, 1, 0], [-1, 0, 0]], [0, 0, 5, 0, 60, 10], 1, 30]], [4.4012, 5.105, 6.0573]]], [['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]]]]
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
identity pose exact 13	`[13.0, 13.0, 13.0]`	`[13.0, 13.0, 13.0]`	Passed
upper bound exact	`[13.0, 13.0]`	`[13.0, 13.0]`	Passed
roll only	`[10.3846, 10.3846, 11.0564]`	`[10.3846, 10.3846, 11.0564]`	Passed
pitch only	`[9.6202, 8.7617, 9.6202]`	`[9.6202, 8.7617, 9.6202]`	Passed
yaw only	`[10.0995, 10.2766, 10.0801]`	`[10.0995, 10.2766, 10.0801]`	Passed
combined attitude	`[9.2584, 7.7756, 8.311]`	`[9.2131, 8.0316, 8.1298]`	Failed
roll and pitch	`[6.0776, 6.7798, 5.9191]`	`[5.9887, 6.7734, 5.9691]`	Failed

SHA-256 / 364bfd9a9e713b40477188ba72f6b945612b85a9b1322063aed31d44543ed184

2 / The unsuccessful fix

Exit 1

"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
    A,B,pose,lmin,lmax=x
    px,py,pz,ro,pi_,ya=pose
    cr,sr=math.cos(math.radians(ro)),math.sin(math.radians(ro))
    cp,sp=math.cos(math.radians(pi_)),math.sin(math.radians(pi_))
    cy,sy=math.cos(math.radians(ya)),math.sin(math.radians(ya))
    Rx=[[1,0,0],[0,cr,-sr],[0,sr,cr]]
    Ry=[[cp,0,sp],[0,1,0],[-sp,0,cp]]
    Rz=[[cy,-sy,0],[sy,cy,0],[0,0,1]]
    mm=lambda P,Q:[[sum(P[i][k]*Q[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
    Rm=mm(mm(Rz,Rx),Ry)
    out=[]
    for i,(a,b) in enumerate(zip(A,B)):
        w=[sum(Rm[r][c]*b[c] for c in range(3)) for r in range(3)]
        v=[px+w[0]-a[0],py+w[1]-a[1],pz+w[2]-a[2]]
        L=math.sqrt(v[0]**2+v[1]**2+v[2]**2)
        if L<lmin or L>lmax: return ['limit',i,round(L,4)]
        out.append(round(L,4))
    return out
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['upper bound exact', [[[[0, 0, 0], [1, 0, 0]], [[3, 4, 0], [4, 4, 0]], [0, 0, 12, 0, 0, 0], 5, 13]], [13.0, 13.0]], ['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]]], [['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]], ['long leg second', [[[[0, 0, 0], [8, 0, 0]], [[0, 0, 0], [0, 0, 0]], [0, 0, 6, 0, 0, 0], 1, 9]], ['limit', 1, 10.0]]], [['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]], ['long leg second', [[[[0, 0, 0], [8, 0, 0]], [[0, 0, 0], [0, 0, 0]], [0, 0, 6, 0, 0, 0], 1, 9]], ['limit', 1, 10.0]], ['yaw with offsets', [[[[3, 0, 0], [-3, 0, 0]], [[1, 0, 0], [-1, 0, 0]], [1, 2, 7, 5, 5, 60], 1, 30]], [7.6314, 7.9865]], ['large pitch', [[[[2, 0, 0], [0, 2, 0], [-2, 0, 0]], [[1, 0, 0], [0, 1, 0], [-1, 0, 0]], [0, 0, 5, 0, 60, 10], 1, 30]], [4.4012, 5.105, 6.0573]]], [['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['upper bound exact', [[[[0, 0, 0], [1, 0, 0]], [[3, 4, 0], [4, 4, 0]], [0, 0, 12, 0, 0, 0], 5, 13]], [13.0, 13.0]], ['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['yaw with offsets', [[[[3, 0, 0], [-3, 0, 0]], [[1, 0, 0], [-1, 0, 0]], [1, 2, 7, 5, 5, 60], 1, 30]], [7.6314, 7.9865]], ['large pitch', [[[[2, 0, 0], [0, 2, 0], [-2, 0, 0]], [[1, 0, 0], [0, 1, 0], [-1, 0, 0]], [0, 0, 5, 0, 60, 10], 1, 30]], [4.4012, 5.105, 6.0573]]], [['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]]]]
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
identity pose exact 13	`[13.0, 13.0, 13.0]`	`[13.0, 13.0, 13.0]`	Passed
upper bound exact	`[13.0, 13.0]`	`[13.0, 13.0]`	Passed
roll only	`[10.3846, 10.3846, 11.0564]`	`[10.3846, 10.3846, 11.0564]`	Passed
pitch only	`[9.6202, 8.7617, 9.6202]`	`[9.6202, 8.7617, 9.6202]`	Passed
yaw only	`[10.0995, 10.2766, 10.0801]`	`[10.0995, 10.2766, 10.0801]`	Passed
combined attitude	`[9.2102, 8.0306, 8.1158]`	`[9.2131, 8.0316, 8.1298]`	Failed
roll and pitch	`[6.0776, 6.7798, 5.9191]`	`[5.9887, 6.7734, 5.9691]`	Failed

SHA-256 / e46f69bcb9825d604db5cd56330e47041dd1115f88d8c636c8d0d16d625d5e25

3 / The verified repair

Exit 0

"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
    A,B,pose,lmin,lmax=x
    px,py,pz,ro,pi_,ya=pose
    cr,sr=math.cos(math.radians(ro)),math.sin(math.radians(ro))
    cp,sp=math.cos(math.radians(pi_)),math.sin(math.radians(pi_))
    cy,sy=math.cos(math.radians(ya)),math.sin(math.radians(ya))
    Rx=[[1,0,0],[0,cr,-sr],[0,sr,cr]]
    Ry=[[cp,0,sp],[0,1,0],[-sp,0,cp]]
    Rz=[[cy,-sy,0],[sy,cy,0],[0,0,1]]
    mm=lambda P,Q:[[sum(P[i][k]*Q[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
    Rm=mm(Rz,mm(Ry,Rx))
    out=[]
    for i,(a,b) in enumerate(zip(A,B)):
        w=[sum(Rm[r][c]*b[c] for c in range(3)) for r in range(3)]
        v=[px+w[0]-a[0],py+w[1]-a[1],pz+w[2]-a[2]]
        L=math.sqrt(v[0]**2+v[1]**2+v[2]**2)
        if L<lmin or L>lmax: return ['limit',i,round(L,4)]
        out.append(round(L,4))
    return out
def check(label, actual, expected):
    observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['upper bound exact', [[[[0, 0, 0], [1, 0, 0]], [[3, 4, 0], [4, 4, 0]], [0, 0, 12, 0, 0, 0], 5, 13]], [13.0, 13.0]], ['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]]], [['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]], ['long leg second', [[[[0, 0, 0], [8, 0, 0]], [[0, 0, 0], [0, 0, 0]], [0, 0, 6, 0, 0, 0], 1, 9]], ['limit', 1, 10.0]]], [['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]], ['long leg second', [[[[0, 0, 0], [8, 0, 0]], [[0, 0, 0], [0, 0, 0]], [0, 0, 6, 0, 0, 0], 1, 9]], ['limit', 1, 10.0]], ['yaw with offsets', [[[[3, 0, 0], [-3, 0, 0]], [[1, 0, 0], [-1, 0, 0]], [1, 2, 7, 5, 5, 60], 1, 30]], [7.6314, 7.9865]], ['large pitch', [[[[2, 0, 0], [0, 2, 0], [-2, 0, 0]], [[1, 0, 0], [0, 1, 0], [-1, 0, 0]], [0, 0, 5, 0, 60, 10], 1, 30]], [4.4012, 5.105, 6.0573]]], [['identity pose exact 13', [[[[0, 0, 0], [3, 0, 0], [0, 3, 0]], [[3, 4, 0], [6, 4, 0], [3, 7, 0]], [0, 0, 12, 0, 0, 0], 13, 20]], [13.0, 13.0, 13.0]], ['upper bound exact', [[[[0, 0, 0], [1, 0, 0]], [[3, 4, 0], [4, 4, 0]], [0, 0, 12, 0, 0, 0], 5, 13]], [13.0, 13.0]], ['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['yaw with offsets', [[[[3, 0, 0], [-3, 0, 0]], [[1, 0, 0], [-1, 0, 0]], [1, 2, 7, 5, 5, 60], 1, 30]], [7.6314, 7.9865]], ['large pitch', [[[[2, 0, 0], [0, 2, 0], [-2, 0, 0]], [[1, 0, 0], [0, 1, 0], [-1, 0, 0]], [0, 0, 5, 0, 60, 10], 1, 30]], [4.4012, 5.105, 6.0573]]], [['roll only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[0, 1, 0], [2, 1, 0], [0, 3, 0]], [0, 0, 10, 20, 0, 0], 1, 30]], [10.3846, 10.3846, 11.0564]], ['pitch only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 0, 0], [3, 0, 0], [1, 2, 0]], [0, 0, 10, 0, 25, 0], 1, 30]], [9.6202, 8.7617, 9.6202]], ['yaw only', [[[[0, 0, 0], [2, 0, 0], [0, 2, 0]], [[1, 1, 0], [3, 1, 0], [1, 3, 0]], [0, 0, 10, 0, 0, 30], 1, 30]], [10.0995, 10.2766, 10.0801]], ['combined attitude', [[[[1, 0, 0], [-1, 1, 0], [-1, -1, 0]], [[1, 0, 1], [-1, 1, 0], [0, -1, 0]], [0.5, -0.5, 8, 10, -15, 40], 1, 30]], [9.2131, 8.0316, 8.1298]], ['roll and pitch', [[[[2, 0, 0], [-1, 2, 0], [-1, -2, 0]], [[1, 0.5, 0], [-0.5, 1, 0], [-0.5, -1, 0.2]], [0, 0, 6, 30, 20, 0], 1, 30]], [5.9887, 6.7734, 5.9691]], ['several legs violate', [[[[0, 0, 0], [5, 0, 0], [0, 5, 0]], [[0, 0, 0], [1, 0, 0], [0, 1, 0]], [0, 0, 4, 0, 0, 0], 4.5, 20]], ['limit', 0, 4.0]], ['short leg first', [[[[0, 0, 0], [1, 0, 0]], [[0, 0, 0], [1, 0, 0]], [0, 0, 2, 0, 0, 0], 3, 10]], ['limit', 0, 2.0]]]]
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
identity pose exact 13	`[13.0, 13.0, 13.0]`	`[13.0, 13.0, 13.0]`	Passed
upper bound exact	`[13.0, 13.0]`	`[13.0, 13.0]`	Passed
roll only	`[10.3846, 10.3846, 11.0564]`	`[10.3846, 10.3846, 11.0564]`	Passed
pitch only	`[9.6202, 8.7617, 9.6202]`	`[9.6202, 8.7617, 9.6202]`	Passed
yaw only	`[10.0995, 10.2766, 10.0801]`	`[10.0995, 10.2766, 10.0801]`	Passed
combined attitude	`[9.2131, 8.0316, 8.1298]`	`[9.2131, 8.0316, 8.1298]`	Passed
roll and pitch	`[5.9887, 6.7734, 5.9691]`	`[5.9887, 6.7734, 5.9691]`	Passed

SHA-256 / fc49f34e3256b6d111aa0dd8a7e0308a096ca13810c621f090b2fec2d91dfc0d

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

Case digest / 288f5d04ad480d38bb22b2681cc8ca79bea8c32a8d579a6386499da33c9ace59