FA-88141 / Inverse kinematics solvers / Open access

Stewart pose vector is unpacked as yaw, pitch, roll · case 01

Roll and yaw commands are exchanged.

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

ROOT CAUSE

The pose is read as [x,y,z,yaw,pitch,roll] although the contract lists roll first.

VERIFIED REPAIR

Unpack [x,y,z,roll,pitch,yaw].

Unsuccessful approach: Swapping roll and pitch instead mixes a different pair.

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,ya,pi_,ro=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]], ['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]], ['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]], ['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]], ['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]], ['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]]]]
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.0499, 10.1415, 10.0858]`	`[10.3846, 10.3846, 11.0564]`	Failed
pitch only	`[9.6202, 8.7617, 9.6202]`	`[9.6202, 8.7617, 9.6202]`	Passed
yaw only	`[10.583, 10.583, 11.5589]`	`[10.0995, 10.2766, 10.0801]`	Failed
combined attitude	`[9.0616, 8.4188, 7.5981]`	`[9.2131, 8.0316, 8.1298]`	Failed
roll and pitch	`[5.9068, 6.3217, 6.5296]`	`[5.9887, 6.7734, 5.9691]`	Failed

SHA-256 / b6e781af73d825fc27bab99a99f9b970b4f4824580a36b3a549fb6ba1293425a

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,pi_,ro,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]], ['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]], ['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]], ['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]], ['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]], ['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]]]]
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.0499, 9.3703, 10.0499]`	`[10.3846, 10.3846, 11.0564]`	Failed
pitch only	`[10.0499, 10.0499, 10.8929]`	`[9.6202, 8.7617, 9.6202]`	Failed
yaw only	`[10.0995, 10.2766, 10.0801]`	`[10.0995, 10.2766, 10.0801]`	Passed
combined attitude	`[8.7912, 8.0459, 8.5342]`	`[9.2131, 8.0316, 8.1298]`	Failed
roll and pitch	`[5.7638, 6.6725, 6.2158]`	`[5.9887, 6.7734, 5.9691]`	Failed

SHA-256 / 47fc5763c5773737bdebbe18a3e9c871c99d0af3efca46074dd2bfca891bc8bf

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

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

Case digest / 5461671e8b52c607a4d93e687ca9ba6e07a79a9a81031474ff5dcf66b060681f