FA-88126 / Inverse kinematics solvers / Open access

Stewart platform anchors are rotated by the inverse attitude · case 01

Legs are computed for the platform rotated the opposite way.

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

ROOT CAUSE

The matrix-vector product reads Rm[c][r], applying R transpose to platform anchors.

VERIFIED REPAIR

Rotate platform anchors with R, row by column.

Unsuccessful approach: Rotating only the horizontal anchor components ignores roll and pitch lift.

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(Rz,mm(Ry,Rx))
    out=[]
    for i,(a,b) in enumerate(zip(A,B)):
        w=[sum(Rm[c][r]*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]], ['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]], ['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]], ['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]], ['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]], ['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]]]]
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	`[9.7036, 9.7036, 9.0112]`	`[10.3846, 10.3846, 11.0564]`	Failed
pitch only	`[10.4619, 11.2908, 10.4619]`	`[9.6202, 8.7617, 9.6202]`	Failed
yaw only	`[10.0995, 10.0801, 10.2766]`	`[10.0995, 10.2766, 10.0801]`	Failed
combined attitude	`[8.9377, 7.909, 8.3464]`	`[9.2131, 8.0316, 8.1298]`	Failed
roll and pitch	`[6.1681, 5.5146, 6.6302]`	`[5.9887, 6.7734, 5.9691]`	Failed

SHA-256 / e5581ea4de372b13cf623b028e6194330e5013823d46c55f60197887921da588

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(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(2)) for r in range(2)]+[b[2]]
        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]], ['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]], ['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]], ['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]], ['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]], ['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]]]]
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.0441, 10.0441, 10.0335]`	`[10.3846, 10.3846, 11.0564]`	Failed
pitch only	`[10.041, 10.0258, 10.041]`	`[9.6202, 8.7617, 9.6202]`	Failed
yaw only	`[10.0995, 10.2766, 10.0801]`	`[10.0995, 10.2766, 10.0801]`	Passed
combined attitude	`[9.004, 8.1213, 8.2915]`	`[9.2131, 8.0316, 8.1298]`	Failed
roll and pitch	`[6.0941, 6.1463, 6.3131]`	`[5.9887, 6.7734, 5.9691]`	Failed

SHA-256 / 1eddf2191af07d3971534f74808eeb7894b2fd1b40a28dea181518bcac504691

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]], ['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]], ['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]], ['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]], ['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]], ['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]]]]
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 / d51b590964388bad95476fd5833bd3137fc08bcb135b3a59815dd0049c5e66c4

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

Case digest / 24de1c3cbfd62a40b2626f119a25d3ea57651771c9569aac120a61b6c48ef665