"""CANONICAL TCP definition (Cameron 2026-07-14): defined RELATIVE to the fixed ENPIRE finger mesh
(shared by the wand and the arm gripper), so every variant derives the identical point.
OFFSET calibrated once from Cameron's Blender-placed tcp cube: tcp (-157.0, -7.7, 24.8) minus the
fixed-finger reference (x_min, y_center, z_min) at calibration time."""
import numpy as np, trimesh

TCP_OFFSET = np.array([18.65, -4.61, 7.11])

def _ref(mesh):
    lo, hi = mesh.bounds
    return np.array([lo[0], (lo[1] + hi[1]) / 2, lo[2]])

def tcp_servo_mm(fixed_finger_stl="gnode_v2_enpire_fixed.stl"):
    m = trimesh.load(fixed_finger_stl, force="mesh")
    return _ref(m) + TCP_OFFSET

if __name__ == "__main__":
    # one-time calibration against Cameron's placed point
    m = trimesh.load("gnode_v2_enpire_fixed.stl", force="mesh")
    placed = np.array([-157.0, -7.7, 24.8])
    off = placed - _ref(m)
    print("reference:", np.round(_ref(m), 2).tolist())
    print("TCP_OFFSET = np.array([%.2f, %.2f, %.2f])" % tuple(off))