import numpy as np, gen_two_servo as g
from scipy.spatial.transform import Rotation as R
lay = g.read_layout("two_servo_from_blender.glb")
prev = None
for i, M in enumerate(lay):
    pos = M[:3, 3] * 1000
    rpy = R.from_matrix(M[:3, :3]).as_euler("xyz", degrees=True)
    snap = np.round(rpy / 90.0) * 90.0
    dev = float(np.abs(((rpy - snap + 180) % 360) - 180).max())
    print(f"servo_{i}: pos={np.round(pos,1).tolist()}  rpy={np.round(rpy,1).tolist()}  ->90={snap.tolist()}  ORIENT_DEV={dev:.1f}deg")
    if prev is not None:
        off = pos - prev
        ax = int(np.argmax(np.abs(off)))
        oth = [(k, round(float(off[k]), 1)) for k in range(3) if k != ax]
        print(f"    connector offset={np.round(off,1).tolist()}  dominant=ax{ax}({off[ax]:.0f}mm)  off-axis={oth}")
    prev = pos