"""Articulated gripper URDF (GLB world frame == servo frame; output axis x=-25.5mm, +Y).
base (static): servo + holder + servo_to_finger + connector1 + fixed_conn + finger_adj.
jaw revolute -> jaw_link (moving): horn_mount (4-hole plate) + yoke_conn + fin-ray finger."""
import numpy as np, trimesh, shutil
import xml.etree.ElementTree as ET
AX = -0.0255
s = trimesh.load("gripper.glb")
COLS = {"servo":"0.35 0.35 0.40 1","holder":"0.20 0.52 0.85 1","servo_to_finger":"0.55 0.60 0.70 1",
        "finger_adj":"0.55 0.60 0.70 1","connector1":"0.95 0.45 0.12 1","fixed_conn":"0.30 0.66 0.45 1",
        "finger":"0.90 0.65 0.20 1","yoke":"0.90 0.65 0.20 1","yoke_conn":"0.85 0.30 0.30 1"}
MOVING = {"yoke", "finger", "yoke_conn"}
GEN = ("yoke_conn", "fixed_conn", "horn_mount", "yoke")            # generated/replaced pieces, never from the GLB
for node in s.graph.nodes_geometry:
    if node in GEN: continue
    T, gn = s.graph[node]
    m = s.geometry[gn].copy(); m.apply_transform(np.array(T, float)); m.apply_scale(100.0)
    m.export(f"gnode_{node}.stl")
shutil.copy("yoke_fillet.stl", "gnode_yoke.stl")                      # REAL yoke: clamps horn + idler (both sides)
nodes = [n for n in s.graph.nodes_geometry if n not in GEN] + ["yoke", "yoke_conn", "fixed_conn"]    # new procedural pieces
def vis(link, node, off="0 0 0"):
    v = ET.SubElement(link, "visual"); ET.SubElement(v, "origin", {"xyz": off, "rpy": "0 0 0"})
    ET.SubElement(ET.SubElement(v, "geometry"), "mesh", {"filename": f"gnode_{node}.stl", "scale": "0.001 0.001 0.001"})
    ET.SubElement(ET.SubElement(v, "material", {"name": "m"+node}), "color", {"rgba": COLS.get(node, "0.6 0.6 0.6 1")})
R = ET.Element("robot", {"name": "gripper"}); ET.SubElement(R, "link", {"name": "world"})
wj = ET.SubElement(R, "joint", {"name": "world_to_base", "type": "fixed"})
ET.SubElement(wj, "parent", {"link": "world"}); ET.SubElement(wj, "child", {"link": "base"})
ET.SubElement(wj, "origin", {"xyz": "0 0 0", "rpy": "1.5708 0 0"})
base = ET.SubElement(R, "link", {"name": "base"})
for node in nodes:
    if node not in MOVING: vis(base, node)
jj = ET.SubElement(R, "joint", {"name": "jaw", "type": "revolute"})
ET.SubElement(jj, "parent", {"link": "base"}); ET.SubElement(jj, "child", {"link": "jaw_link"})
ET.SubElement(jj, "origin", {"xyz": f"{AX} 0 0", "rpy": "0 0 0"}); ET.SubElement(jj, "axis", {"xyz": "0 1 0"})
ET.SubElement(jj, "limit", {"lower": "-0.1", "upper": "0.9", "effort": "3", "velocity": "3"})
jl = ET.SubElement(R, "link", {"name": "jaw_link"})
for node in nodes:
    if node in MOVING: vis(jl, node, off=f"{-AX} 0 0")
ET.indent(ET.ElementTree(R), "  "); ET.ElementTree(R).write("gripper_testing.urdf", encoding="unicode", xml_declaration=True)
# GLB for Blender round-trip: original nodes + the new pieces as editable nodes
out = trimesh.Scene()
for node in nodes:
    m = trimesh.load(f"gnode_{node}.stl", force="mesh"); m.apply_scale(0.01)
    out.add_geometry(m, node_name=node, geom_name=node)
out.export("gripper.glb")
print("wrote gripper_testing.urdf + gripper.glb; moving:", sorted(MOVING))