"""Access-gantry on a mobile base: 4-wheel chassis + a compact 3-axis gantry portal on top + tool.
The whole XYZ workspace rides around. Y-up model, world joint -> wheels on ground."""
import xml.etree.ElementTree as ET
R=ET.Element("robot",{"name":"mobile_gantry"})
def link(n): return ET.SubElement(R,"link",{"name":n})
def mat(v,rgba,k): ET.SubElement(ET.SubElement(v,"material",{"name":"m"+k}),"color",{"rgba":rgba})
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});mat(v,rgba,size+xyz)
def cvis(lk,r,l,xyz,rgba,rpy):
    v=ET.SubElement(lk,"visual");ET.SubElement(v,"origin",{"xyz":xyz,"rpy":rpy});ET.SubElement(ET.SubElement(v,"geometry"),"cylinder",{"radius":r,"length":l});mat(v,rgba,r+xyz)
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":"300","velocity":"1"})
CH="0.30 0.32 0.36 1"; WH="0.1 0.1 0.11 1"; EX="0.62 0.62 0.66 1"; DK="0.26 0.26 0.30 1"; TL="0.9 0.52 0.16 1"; MT="0.15 0.15 0.17 1"
WR=0.05
link("world"); joint("world_up","fixed","world","base_link","0 0 0",rpy="1.5708 0 0")
base=link("base_link")
bvis(base,"0.45 0.10 0.35","0 0.10 0",CH)                              # chassis
for x in (-0.18,0.18): bvis(base,"0.04 0.50 0.04",f"{x} 0.40 0",EX)   # 2 gantry legs on top of chassis
bvis(base,"0.44 0.04 0.04","0 0.65 0",EX)                             # top X rail
for i,(x,z) in enumerate([(0.20,0.15),(-0.20,0.15),(0.20,-0.15),(-0.20,-0.15)]):
    w=link(f"wheel{i}"); cvis(w,str(WR),"0.03","0 0 0",WH,"0 1.5708 0"); joint(f"j_wheel{i}","continuous","base_link",f"wheel{i}",f"{x} {WR} {z}",axis="1 0 0")
# gantry X -> Z -> Y(drop) -> tool
xc=link("gx"); bvis(xc,"0.06 0.05 0.06","0 0 0",DK); bvis(xc,"0.04 0.04 0.36","0 -0.04 0",EX); bvis(xc,"0.06 0.04 0.06","0.06 0.02 0",MT)
joint("g_X","prismatic","base_link","gx","0 0.65 0",axis="1 0 0",lo=-0.15,hi=0.15)
zc=link("gz"); bvis(zc,"0.05 0.05 0.05","0 0 0",DK); bvis(zc,"0.03 0.34 0.03","0 -0.17 0",EX)
joint("g_Z","prismatic","gx","gz","0 -0.04 0",axis="0 0 1",lo=-0.13,hi=0.13)
yc=link("gy"); bvis(yc,"0.06 0.05 0.06","0 0 0",DK)
joint("g_Y","prismatic","gz","gy","0 -0.05 0",axis="0 1 0",lo=-0.32,hi=0.0)
tool=link("gtool"); bvis(tool,"0.035 0.05 0.035","0 -0.04 0",TL)
joint("g_tool","fixed","gy","gtool","0 -0.05 0")
ET.indent(ET.ElementTree(R),"  "); ET.ElementTree(R).write("mobile_gantry.urdf",encoding="unicode",xml_declaration=True)
print("wrote mobile_gantry.urdf")