"""Base variant: HANDLE COLUMN instead of the finger cradle (same position, fixed/servo side).
Full-hand grasp on a O30 column; thumb cradle (moving side) unchanged. Outputs
gripper_v2_base_v3_handle.stl + umi_gripper_v3_handle.urdf."""
import numpy as np, trimesh, trimesh.repair
import xml.etree.ElementTree as ET

HX, HZ = -48.0, 42.3          # Cameron-placed (Blender 2026-07-09)
HR = 9.0                      # O18: 3/5 of the original O30 (Cameron 2026-07-09)
TOP_Y, LEN = 6.0, 95.0        # buried deeper per his placement; ~95mm grip length
SPLIT_Y = -28.7
PIN_R, PIN_PROUD = 1.5, 3.5
def LR(f):
    m = trimesh.load(f, force="mesh")
    if not m.is_volume:
        m.merge_vertices(merge_tex=True, merge_norm=True)
        m.update_faces(m.nondegenerate_faces()); m.update_faces(m.unique_faces())
        trimesh.repair.fill_holes(m); m.fix_normals()
    return m
def U(parts):
    m = trimesh.boolean.union(parts, engine="manifold")
    keep = [b for b in m.split(only_watertight=False) if b.volume > 100.0]
    return keep[0] if len(keep) == 1 else trimesh.util.concatenate(keep)
def cylY(x, z, r, y0, y1, sec=48):
    c = trimesh.creation.cylinder(radius=r, height=abs(y1 - y0), sections=sec)
    c.apply_transform(trimesh.transformations.rotation_matrix(np.pi / 2, [1, 0, 0]))
    c.apply_translation([x, (y0 + y1) / 2, z]); return c

shaft = cylY(HX, HZ, HR, TOP_Y, TOP_Y + LEN - HR)
tip = trimesh.creation.icosphere(radius=HR, subdivisions=3)
tip.apply_translation([HX, TOP_Y + LEN - HR, HZ])
flare = trimesh.creation.cylinder(radius=HR + 4.0, height=6.0, sections=48)   # root flare into the plate
flare.apply_transform(trimesh.transformations.rotation_matrix(np.pi / 2, [1, 0, 0]))
flare.apply_translation([HX, TOP_Y + 3.0, HZ])
handle = U([shaft, tip, flare])
# clearance audits
servo_hull = trimesh.PointCloud(trimesh.load("st3215.stl", force="mesh").vertices).convex_hull
for tn, tm in [("servo", servo_hull), ("thumb_cradle", LR("gnode_v2_thumb_cradle.stl"))]:
    pts, _ = trimesh.sample.sample_surface(handle, 6000)
    pen = float(trimesh.proximity.ProximityQuery(tm).signed_distance(pts).max())
    print(f"handle vs {tn}: {'PENETRATES %.1f' % pen if pen > 0.05 else 'clear'}")
pins = [(-1.0, -8.0, -28.3), (-1.0, 8.0, -28.3), (14.0, 20.0, -28.6), (14.0, 33.0, -28.6)]
pin_meshes = []
for (px, pz, ytop) in pins:
    pin_meshes.append(cylY(px, pz, PIN_R, ytop + 2.0, SPLIT_Y - PIN_PROUD + PIN_R, sec=40))
    d = trimesh.creation.icosphere(radius=PIN_R, subdivisions=3)
    d.apply_translation([px, SPLIT_Y - PIN_PROUD + PIN_R, pz]); pin_meshes.append(d)
handle.export("gnode_v2_handle_column.stl")
base = U([LR("gnode_v2_holder.stl"), LR("gnode_v2_fixed_riser.stl"), LR("gnode_v2_fixed_plate.stl"),
          LR("gnode_v2_enpire_fixed.stl"), handle] + pin_meshes)
base.export("gripper_v2_base_v3_handle.stl")
print(f"base_v3_handle: wt={base.is_watertight} bodies={len(base.split(only_watertight=False))} vol={base.volume/1000:.1f}cm3")
t = ET.parse("umi_gripper_v3.urdf")
for mesh in t.getroot().iter("mesh"):
    if mesh.get("filename") == "gripper_v2_base_v3.stl":
        mesh.set("filename", "gripper_v2_base_v3_handle.stl")
t.getroot().set("name", "umi_v3_handle")
t.write("umi_gripper_v3_handle.urdf", encoding="unicode", xml_declaration=True)
print("wrote umi_gripper_v3_handle.urdf")