import numpy as np, trimesh, xml.etree.ElementTree as ET
s=trimesh.load("gripper_from_blender.glb")
parts=[]
for node in s.graph.nodes_geometry:
    if node.startswith("connector"): continue
    T,gn=s.graph[node]
    m=s.geometry[gn].copy(); m.apply_transform(np.array(T,float))
    parts.append(m)
p=trimesh.util.concatenate(parts); p.apply_scale(100.0); p.export("gripper_parts.stl")
R=ET.Element("robot",{"name":"gc"}); ET.SubElement(R,"link",{"name":"world"})
j=ET.SubElement(R,"joint",{"name":"w","type":"fixed"}); ET.SubElement(j,"parent",{"link":"world"}); ET.SubElement(j,"child",{"link":"g"}); ET.SubElement(j,"origin",{"xyz":"0 0 0","rpy":"1.5708 0 0"})
L=ET.SubElement(R,"link",{"name":"g"})
for fn,rgba in [("gripper_parts.stl","0.55 0.57 0.62 1"),("gripper_connector1.stl","0.95 0.45 0.12 1"),("gripper_connector2.stl","0.30 0.66 0.45 1")]:
    v=ET.SubElement(L,"visual"); ET.SubElement(v,"origin",{"xyz":"0 0 0","rpy":"0 0 0"})
    ET.SubElement(ET.SubElement(v,"geometry"),"mesh",{"filename":fn,"scale":"0.001 0.001 0.001"})
    ET.SubElement(ET.SubElement(v,"material",{"name":"m"+fn}),"color",{"rgba":rgba})
ET.ElementTree(R).write("gripper_assembled.urdf"); print("ok")