import xml.etree.ElementTree as ET
R=ET.Element("robot",{"name":"gantry_arm"})
def link(n): return ET.SubElement(R,"link",{"name":n})
def bvis(lk,size,xyz,rgba,rpy="0 0 0"):
    v=ET.SubElement(lk,"visual");ET.SubElement(v,"origin",{"xyz":xyz,"rpy":rpy})
    ET.SubElement(ET.SubElement(v,"geometry"),"box",{"size":size})
    ET.SubElement(ET.SubElement(v,"material",{"name":"m"+str(id(v))}),"color",{"rgba":rgba})
def mvis(lk,fn,xyz,rgba,rpy="0 0 0"):
    v=ET.SubElement(lk,"visual");ET.SubElement(v,"origin",{"xyz":xyz,"rpy":rpy})
    ET.SubElement(ET.SubElement(v,"geometry"),"mesh",{"filename":fn,"scale":"0.001 0.001 0.001"})
    ET.SubElement(ET.SubElement(v,"material",{"name":"m"+str(id(v))}),"color",{"rgba":rgba})
def joint(n,typ,par,ch,xyz,axis=None,lo=None,hi=None,rpy="0 0 0"):
    j=ET.SubElement(R,"joint",{"name":n,"type":typ})
    ET.SubElement(j,"parent",{"link":par});ET.SubElement(j,"child",{"link":ch})
    ET.SubElement(j,"origin",{"xyz":xyz,"rpy":rpy})
    if axis:ET.SubElement(j,"axis",{"xyz":axis})
    if lo is not None:ET.SubElement(j,"limit",{"lower":str(lo),"upper":str(hi),"effort":"200","velocity":"1"})
GR="0.62 0.62 0.66 1"; DK="0.26 0.26 0.30 1"; SV="0.34 0.34 0.40 1"; TL="0.90 0.52 0.16 1"
H=1.0                                                # leg height (taller)
link("world")
joint("world_up","fixed","world","base_link","0 0 0",rpy="1.5708 0 0")   # Y-up model -> Z-up display, feet on ground
base=link("base_link")
for x in (-0.25,0.25): bvis(base,f"0.04 {H} 0.04",f"{x} {H/2} 0",GR)      # 2 vertical legs (full length above ground)
bvis(base,f"0.58 0.04 0.04",f"0 {H} 0",GR)                               # top horizontal rail = X axis
bvis(base,"0.54 0.04 0.04","0 0.02 0",GR)                                # bottom cross-bar ON the ground
for x in (-0.25,0.25): bvis(base,"0.16 0.02 0.18",f"{x} 0.01 0",DK)      # feet
xc=link("x_carriage"); bvis(xc,"0.07 0.05 0.07","0 0 0",DK); bvis(xc,"0.04 0.04 0.54","0 -0.045 0",GR)  # rail2 = Z
joint("axis_X","prismatic","base_link","x_carriage",f"0 {H} 0",axis="1 0 0",lo=-0.22,hi=0.22)
zc=link("z_carriage"); bvis(zc,"0.06 0.05 0.06","0 0 0",DK); bvis(zc,"0.04 0.5 0.04","0 -0.25 0",GR)    # rail3 = Y (vertical drop)
joint("axis_Z","prismatic","x_carriage","z_carriage","0 -0.045 0",axis="0 0 1",lo=-0.22,hi=0.22)
yc=link("y_carriage"); bvis(yc,"0.07 0.06 0.07","0 0 0",DK)
joint("axis_Y","prismatic","z_carriage","y_carriage","0 -0.06 0",axis="0 1 0",lo=-0.42,hi=0.0)
prev="y_carriage"
for i,(nm,ax) in enumerate([("arm1","0 1 0"),("arm2","1 0 0"),("arm3","1 0 0"),("arm4","0 1 0")]):
    lk=link(nm); mvis(lk,"st3215.stl","0 -0.015 0",SV,rpy="1.5708 0 0")
    joint(f"j_{nm}","revolute",prev,nm,"0 -0.05 0" if i==0 else "0 -0.052 0",axis=ax,lo=-1.57,hi=1.57); prev=nm
tool=link("tool"); bvis(tool,"0.03 0.035 0.05","0 -0.03 0",TL)
joint("j_tool","fixed",prev,"tool","0 -0.05 0")
ET.indent(ET.ElementTree(R),"  "); ET.ElementTree(R).write("gantry_arm.urdf",encoding="unicode",xml_declaration=True)
print(f"wrote gantry_arm.urdf  legs={H}m tall, feet on ground, arm hangs in working volume")