FA-88161 / Inverse kinematics solvers / Open access
Hexapod femur triangle angle is taken at the foot · case 01
Femur pitch is wrong whenever femur and tibia differ in length.
ROOT CAUSE
The law of cosines uses the tibia as the adjacent side, giving the angle at the foot vertex.
VERIFIED REPAIR
Use (femur^2 + D^2 - tibia^2)/(2 femur D).
Unsuccessful approach: Dividing by 2 femur tibia uses the wrong adjacent side product.
Case contract
Input [coxa,femur,tibia,x,y,z,fmin,fmax]: foot target in the leg root frame (z up). Coxa yaw is atan2(y,x); the femur/tibia pair works in the vertical plane at horizontal distance hypot(x,y)-coxa. With D the distance from the femur pivot to the foot, D>femur+tibia or D<|femur-tibia| or D==0 is "unreachable". Femur pitch is atan2(z,h) plus the triangle angle at the femur pivot (knee up); tibia angle is the interior knee angle minus 180 (0 = straight). Femur pitch outside [fmin,fmax] returns "limit". Degrees rounded to 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):
cx,fe,ti,px,py,pz,fmin,fmax=x
ca=math.atan2(py,px)
h=math.hypot(px,py)-cx
D=math.hypot(h,pz)
if D>fe+ti or D<abs(fe-ti) or D==0: return 'unreachable'
a1=math.atan2(pz,h)
a2=math.acos(max(-1.0,min(1.0,(ti*ti+D*D-fe*fe)/(2*ti*D))))
fa=math.degrees(a1+a2)
ta=math.degrees(math.acos(max(-1.0,min(1.0,(fe*fe+ti*ti-D*D)/(2*fe*ti)))))-180.0
if fa<fmin or fa>fmax: return 'limit'
return [round(math.degrees(ca),4),round(fa,4),round(ta,4)]
def check(label, actual, expected):
observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['foot out of reach', [[3, 6, 8, 18, 0, -2, -90, 90]], 'unreachable'], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['high foot', [[3, 6, 8, 9, 3, 4, -90, 90]], 'limit']], [['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot out of reach', [[3, 6, 8, 18, 0, -2, -90, 90]], 'unreachable'], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['high foot', [[3, 6, 8, 9, 3, 4, -90, 90]], 'limit'], ['femur limit exceeded', [[3, 6, 8, 5, 0, 6, -90, 30]], 'limit'], ['deep foot', [[3, 6, 8, 5, 5, -9, -90, 90]], [45.0, -11.6013, -91.4483]], ['tight femur limits', [[3, 6, 8, 10, 2, -6, -10, 10]], 'limit']], [['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['femur limit exceeded', [[3, 6, 8, 5, 0, 6, -90, 30]], 'limit'], ['deep foot', [[3, 6, 8, 5, 5, -9, -90, 90]], [45.0, -11.6013, -91.4483]], ['tight femur limits', [[3, 6, 8, 10, 2, -6, -10, 10]], 'limit'], ['long tibia near', [[2, 4, 9, 6, 1, -8, -90, 90]], [9.4623, 14.4416, -103.1099]], ['diagonal foot', [[2.5, 5, 6, -6, 6, -3, -90, 90]], [135.0, 33.0338, -105.6409]], ['hole too close', [[3, 6, 12, 4, 0, -1, -90, 90]], 'unreachable']], [['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['long tibia near', [[2, 4, 9, 6, 1, -8, -90, 90]], [9.4623, 14.4416, -103.1099]], ['diagonal foot', [[2.5, 5, 6, -6, 6, -3, -90, 90]], [135.0, 33.0338, -105.6409]], ['hole too close', [[3, 6, 12, 4, 0, -1, -90, 90]], 'unreachable'], ['foot far below', [[3, 6, 8, 15, 0, -10, -90, 90]], 'unreachable'], ['femur at lower bound', [[3, 6, 8, 11, 0, -8, -60, 90]], [0.0, -2.44, -73.0422]]], [['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['foot far below', [[3, 6, 8, 15, 0, -10, -90, 90]], 'unreachable'], ['femur at lower bound', [[3, 6, 8, 11, 0, -8, -60, 90]], [0.0, -2.44, -73.0422]]]]
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 |
|---|---|---|---|
| nominal stance | [11.3099, -0.3854, -97.294] | [11.3099, 18.0529, -97.294] | Failed |
| foot straight out | [0.0, 20.5425, -95.9792] | [0.0, 38.5667, -95.9792] | Failed |
| foot to the side | [83.6598, 8.1999, -96.745] | [83.6598, 29.4436, -96.745] | Failed |
| foot behind | [-159.444, -0.1705, -95.0617] | [-159.444, 20.4684, -95.0617] | Failed |
| foot out of reach | unreachable | unreachable | Passed |
| foot inside coxa reach | [0.0, 18.717, -44.0486] | [0.0, 25.3317, -44.0486] | Failed |
| high foot | [18.4349, 76.7539, -115.8921] | limit | Failed |
SHA-256 / 2ef3fdbb795a09d536c3275aef262b081cda706cbb411791cba78a706dedc5e2
2 / The unsuccessful fix
Exit 1"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
cx,fe,ti,px,py,pz,fmin,fmax=x
ca=math.atan2(py,px)
h=math.hypot(px,py)-cx
D=math.hypot(h,pz)
if D>fe+ti or D<abs(fe-ti) or D==0: return 'unreachable'
a1=math.atan2(pz,h)
a2=math.acos(max(-1.0,min(1.0,(fe*fe+D*D-ti*ti)/(2*fe*ti))))
fa=math.degrees(a1+a2)
ta=math.degrees(math.acos(max(-1.0,min(1.0,(fe*fe+ti*ti-D*D)/(2*fe*ti)))))-180.0
if fa<fmin or fa>fmax: return 'limit'
return [round(math.degrees(ca),4),round(fa,4),round(ta,4)]
def check(label, actual, expected):
observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['foot out of reach', [[3, 6, 8, 18, 0, -2, -90, 90]], 'unreachable'], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['high foot', [[3, 6, 8, 9, 3, 4, -90, 90]], 'limit']], [['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot out of reach', [[3, 6, 8, 18, 0, -2, -90, 90]], 'unreachable'], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['high foot', [[3, 6, 8, 9, 3, 4, -90, 90]], 'limit'], ['femur limit exceeded', [[3, 6, 8, 5, 0, 6, -90, 30]], 'limit'], ['deep foot', [[3, 6, 8, 5, 5, -9, -90, 90]], [45.0, -11.6013, -91.4483]], ['tight femur limits', [[3, 6, 8, 10, 2, -6, -10, 10]], 'limit']], [['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['femur limit exceeded', [[3, 6, 8, 5, 0, 6, -90, 30]], 'limit'], ['deep foot', [[3, 6, 8, 5, 5, -9, -90, 90]], [45.0, -11.6013, -91.4483]], ['tight femur limits', [[3, 6, 8, 10, 2, -6, -10, 10]], 'limit'], ['long tibia near', [[2, 4, 9, 6, 1, -8, -90, 90]], [9.4623, 14.4416, -103.1099]], ['diagonal foot', [[2.5, 5, 6, -6, 6, -3, -90, 90]], [135.0, 33.0338, -105.6409]], ['hole too close', [[3, 6, 12, 4, 0, -1, -90, 90]], 'unreachable']], [['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['long tibia near', [[2, 4, 9, 6, 1, -8, -90, 90]], [9.4623, 14.4416, -103.1099]], ['diagonal foot', [[2.5, 5, 6, -6, 6, -3, -90, 90]], [135.0, 33.0338, -105.6409]], ['hole too close', [[3, 6, 12, 4, 0, -1, -90, 90]], 'unreachable'], ['foot far below', [[3, 6, 8, 15, 0, -10, -90, 90]], 'unreachable'], ['femur at lower bound', [[3, 6, 8, 11, 0, -8, -60, 90]], [0.0, -2.44, -73.0422]]], [['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['foot far below', [[3, 6, 8, 15, 0, -10, -90, 90]], 'unreachable'], ['femur at lower bound', [[3, 6, 8, 11, 0, -8, -60, 90]], [0.0, -2.44, -73.0422]]]]
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 |
|---|---|---|---|
| nominal stance | [11.3099, 11.6483, -97.294] | [11.3099, 18.0529, -97.294] | Failed |
| foot straight out | [0.0, 31.3369, -95.9792] | [0.0, 38.5667, -95.9792] | Failed |
| foot to the side | [83.6598, 23.8057, -96.745] | [83.6598, 29.4436, -96.745] | Failed |
| foot behind | [-159.444, 13.8583, -95.0617] | [-159.444, 20.4684, -95.0617] | Failed |
| foot out of reach | unreachable | unreachable | Passed |
| foot inside coxa reach | [0.0, 0.0, -44.0486] | [0.0, 25.3317, -44.0486] | Failed |
| high foot | limit | limit | Passed |
SHA-256 / bbd79f6a13284f8627403e39d8fff17c0007038b0d9092b50f8361b1c8da8955
3 / The verified repair
Exit 0"""Failure Map reference implementation. Python standard library only."""
import json
import math
N = 1
observations = []
def solve(x):
cx,fe,ti,px,py,pz,fmin,fmax=x
ca=math.atan2(py,px)
h=math.hypot(px,py)-cx
D=math.hypot(h,pz)
if D>fe+ti or D<abs(fe-ti) or D==0: return 'unreachable'
a1=math.atan2(pz,h)
a2=math.acos(max(-1.0,min(1.0,(fe*fe+D*D-ti*ti)/(2*fe*D))))
fa=math.degrees(a1+a2)
ta=math.degrees(math.acos(max(-1.0,min(1.0,(fe*fe+ti*ti-D*D)/(2*fe*ti)))))-180.0
if fa<fmin or fa>fmax: return 'limit'
return [round(math.degrees(ca),4),round(fa,4),round(ta,4)]
def check(label, actual, expected):
observations.append({"check": label, "actual": actual, "expected": expected, "passed": actual == expected})
fixtures = [[['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['foot out of reach', [[3, 6, 8, 18, 0, -2, -90, 90]], 'unreachable'], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['high foot', [[3, 6, 8, 9, 3, 4, -90, 90]], 'limit']], [['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot out of reach', [[3, 6, 8, 18, 0, -2, -90, 90]], 'unreachable'], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['high foot', [[3, 6, 8, 9, 3, 4, -90, 90]], 'limit'], ['femur limit exceeded', [[3, 6, 8, 5, 0, 6, -90, 30]], 'limit'], ['deep foot', [[3, 6, 8, 5, 5, -9, -90, 90]], [45.0, -11.6013, -91.4483]], ['tight femur limits', [[3, 6, 8, 10, 2, -6, -10, 10]], 'limit']], [['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['femur limit exceeded', [[3, 6, 8, 5, 0, 6, -90, 30]], 'limit'], ['deep foot', [[3, 6, 8, 5, 5, -9, -90, 90]], [45.0, -11.6013, -91.4483]], ['tight femur limits', [[3, 6, 8, 10, 2, -6, -10, 10]], 'limit'], ['long tibia near', [[2, 4, 9, 6, 1, -8, -90, 90]], [9.4623, 14.4416, -103.1099]], ['diagonal foot', [[2.5, 5, 6, -6, 6, -3, -90, 90]], [135.0, 33.0338, -105.6409]], ['hole too close', [[3, 6, 12, 4, 0, -1, -90, 90]], 'unreachable']], [['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['long tibia near', [[2, 4, 9, 6, 1, -8, -90, 90]], [9.4623, 14.4416, -103.1099]], ['diagonal foot', [[2.5, 5, 6, -6, 6, -3, -90, 90]], [135.0, 33.0338, -105.6409]], ['hole too close', [[3, 6, 12, 4, 0, -1, -90, 90]], 'unreachable'], ['foot far below', [[3, 6, 8, 15, 0, -10, -90, 90]], 'unreachable'], ['femur at lower bound', [[3, 6, 8, 11, 0, -8, -60, 90]], [0.0, -2.44, -73.0422]]], [['nominal stance', [[3, 6, 8, 10, 2, -6, -90, 90]], [11.3099, 18.0529, -97.294]], ['foot straight out', [[3, 6, 8, 12, 0, -3, -90, 90]], [0.0, 38.5667, -95.9792]], ['foot to the side', [[2, 5, 7, 1, 9, -4, -90, 90]], [83.6598, 29.4436, -96.745]], ['foot behind', [[2, 5, 7, -8, -3, -5, -90, 90]], [-159.444, 20.4684, -95.0617]], ['foot inside coxa reach', [[3, 6, 8, 16, 0, 0, -90, 90]], [0.0, 25.3317, -44.0486]], ['foot far below', [[3, 6, 8, 15, 0, -10, -90, 90]], 'unreachable'], ['femur at lower bound', [[3, 6, 8, 11, 0, -8, -60, 90]], [0.0, -2.44, -73.0422]]]]
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 |
|---|---|---|---|
| nominal stance | [11.3099, 18.0529, -97.294] | [11.3099, 18.0529, -97.294] | Passed |
| foot straight out | [0.0, 38.5667, -95.9792] | [0.0, 38.5667, -95.9792] | Passed |
| foot to the side | [83.6598, 29.4436, -96.745] | [83.6598, 29.4436, -96.745] | Passed |
| foot behind | [-159.444, 20.4684, -95.0617] | [-159.444, 20.4684, -95.0617] | Passed |
| foot out of reach | unreachable | unreachable | Passed |
| foot inside coxa reach | [0.0, 25.3317, -44.0486] | [0.0, 25.3317, -44.0486] | Passed |
| high foot | limit | limit | Passed |
SHA-256 / 341ce9afa9adab139d992a4e442101cf05b347c254797f0b86d7443a1941089c
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.595452+00:00.
Case digest / 0cfe2f755195bb1f9e3a5ed2375e761c6fce17faa109703e09fb477c75022bc3