"""umi_gripper_v3.urdf: the SPLIT design — base_v3 + drop-on UMI module (explode slider) + moving jaw."""
import xml.etree.ElementTree as ET
R = ET.Element("robot", {"name": "umi_v3"}); 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"})
def vis(link, fn, rgba, 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": fn, "scale": "0.001 0.001 0.001"})
    ET.SubElement(ET.SubElement(v, "material", {"name": "m" + fn}), "color", {"rgba": rgba})
base = ET.SubElement(R, "link", {"name": "base"})
vis(base, "st3215.stl", "0.35 0.35 0.40 1")
vis(base, "gripper_v2_base_v3.stl", "0.25 0.55 0.85 1")
# UMI module on an explode slider (drop-on direction = -y servo = world up)
j = ET.SubElement(R, "joint", {"name": "explode_module", "type": "prismatic"})
ET.SubElement(j, "parent", {"link": "base"}); ET.SubElement(j, "child", {"link": "umimod"})
ET.SubElement(j, "origin", {"xyz": "0 0 0", "rpy": "0 0 0"}); ET.SubElement(j, "axis", {"xyz": "0 -1 0"})
ET.SubElement(j, "limit", {"lower": "0", "upper": "0.08", "effort": "1", "velocity": "1"})
mod = ET.SubElement(R, "link", {"name": "umimod"})
vis(mod, "gripper_v2_umimod_v3.stl", "0.95 0.60 0.20 1")
vis(mod, "gnode_v2_camera_body.stl", "0.15 0.35 0.15 1")
vis(mod, "gnode_v2_camera_cover.stl", "0.55 0.85 0.65 1")
# moving jaw
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": "-0.0255 0 0", "rpy": "0 0 0"}); ET.SubElement(jj, "axis", {"xyz": "0 1 0"})
ET.SubElement(jj, "limit", {"lower": "-0.9", "upper": "0.1", "effort": "3", "velocity": "3"})
jl = ET.SubElement(R, "link", {"name": "jaw_link"})
vis(jl, "gripper_v2_print_moving.stl", "0.90 0.65 0.20 1", off="0.0255 0 0")
t = ET.ElementTree(R); ET.indent(t, "  ")
t.write("umi_gripper_v3.urdf", encoding="unicode", xml_declaration=True)
print("wrote umi_gripper_v3.urdf")