"""Assemble linear_rail.urdf: base (rail + ends + screw + coupler + STS3215 in holder + YOKE on the output —
the yoke is how anything attaches to the servo) + prismatic carriage (dovetail slider + printed nut)."""
import xml.etree.ElementTree as ET
R=ET.Element("robot",{"name":"linear_rail"})
def link(n): return ET.SubElement(R,"link",{"name":n})
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"+fn+xyz}),"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":"100","velocity":"1"})
RA="0.55 0.57 0.62 1"; SC="0.78 0.74 0.5 1"; EN="0.3 0.6 0.45 1"; SVc="0.34 0.34 0.4 1"; HD="0.2 0.52 0.85 1"; CO="0.85 0.45 0.12 1"
link("world"); joint("world_up","fixed","world","base_link","0 0 0",rpy="1.5708 0 0")
b=link("base_link")
mvis(b,"rail.stl","0 0 0",RA); mvis(b,"motor_end.stl","0 0 0",EN); mvis(b,"idler_end.stl","0 0 0",EN)
mvis(b,"leadscrew.stl","0 0 0",SC); mvis(b,"coupler.stl","0 0 0",CO)
mvis(b,"holder_fillet.stl","0.0255 0.02 -0.105",HD,"1.5708 0 0"); mvis(b,"st3215.stl","0.0255 0.02 -0.105",SVc,"1.5708 0 0"); mvis(b,"yoke_fillet.stl","0.0255 0.02 -0.105","0.85 0.6 0.2 1","1.5708 0 0")
cl=link("carriage"); mvis(cl,"carriage.stl","0 0 0",RA); mvis(cl,"leadnut.stl","0 0 0",CO)
joint("carriage_slide","prismatic","base_link","carriage","0 0 0",axis="0 0 1",lo=-0.06,hi=0.06)
ET.indent(ET.ElementTree(R),"  "); ET.ElementTree(R).write("linear_rail.urdf",encoding="unicode",xml_declaration=True)
print("wrote linear_rail.urdf")