FA-88136 / Inverse kinematics solvers / Open access

Stewart limit report names the last violating leg · case 01

When several legs are out of range the report points at the wrong actuator.

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

ROOT CAUSE

The violation is recorded and overwritten while iterating instead of returning at the first leg.

VERIFIED REPAIR

Return the first leg that violates the range.

Unsuccessful approach: Reporting a one-based leg index mislabels the actuator.

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=[]
    bad=None
    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: bad=['limit',i,round(L,4)]
        out.append(round(L,4))
    return bad if bad else 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]], ['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]]], [['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]], ['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]], ['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]]], [['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]], ['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
several legs violate	`['limit', 0, 4.0]`	`['limit', 0, 4.0]`	Passed
short leg first	`['limit', 1, 2.0]`	`['limit', 0, 2.0]`	Failed

SHA-256 / 63da6d2b7b6604634a3dea3b917178d717924dcc654d0221634693e0610f9e71

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(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+1,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]], ['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]]], [['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]], ['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]], ['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]]], [['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]], ['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
several legs violate	`['limit', 1, 4.0]`	`['limit', 0, 4.0]`	Failed
short leg first	`['limit', 1, 2.0]`	`['limit', 0, 2.0]`	Failed

SHA-256 / fd659ffc82ffbb70fa4c27ef42b35dd69345be4581baf29416ab73a549413300

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]], ['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]]], [['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]], ['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]], ['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]]], [['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]], ['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
several legs violate	`['limit', 0, 4.0]`	`['limit', 0, 4.0]`	Passed
short leg first	`['limit', 0, 2.0]`	`['limit', 0, 2.0]`	Passed

SHA-256 / 0033784999e9fc581a0ee1ab2f1c858c676485c79364a6aa014bc3534940e539

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

Case digest / 7396386850c328a1251ae17ca2df6cb3b69f4d2e8f5cd33464d0ceab6253e265