"""Compose the arm + gripper into one URDF at build time (modular: both sources stay independent).
- arm   = two_servo_beauty.urdf   (the assembled physical arm)
- grip  = umi_gripper_v2.urdf     (v2: ENPIRE fingers + handle + thumb cradle + camera, for manual guiding)
Re-run THIS after either changes. Gripper links/joints get a gripper_ prefix; its world root is dropped and
its base hangs off the arm's last yoke via a fixed joint (ATTACH_* below = the tuning knobs; default butts
the gripper holder face against the yoke_5 arm stub, continuing the chain)."""
import xml.etree.ElementTree as ET
ARM, GRIP, OUT = "two_servo_beauty.urdf", "umi_gripper_v2_arm.urdf", "arm_with_umi_v2.urdf"
ATTACH_PARENT, ATTACH_XYZ, ATTACH_RPY = "yoke_5", "-0.0464 0 0", "0 0 0"
arm = ET.parse(ARM).getroot()
g = ET.parse(GRIP).getroot()
links = [l for l in g.findall("link") if l.get("name") != "world"]
joints = [j for j in g.findall("joint") if j.get("name") != "world_to_base"]
for l in links:
    l.set("name", "gripper_" + l.get("name"))
    for mat in l.iter("material"):
        mat.set("name", "g_" + (mat.get("name") or "m"))
for j in joints:
    j.set("name", "gripper_" + j.get("name"))
    j.find("parent").set("link", "gripper_" + j.find("parent").get("link"))
    j.find("child").set("link", "gripper_" + j.find("child").get("link"))
aj = ET.SubElement(arm, "joint", {"name": "arm_to_gripper", "type": "fixed"})
ET.SubElement(aj, "parent", {"link": ATTACH_PARENT}); ET.SubElement(aj, "child", {"link": "gripper_base"})
ET.SubElement(aj, "origin", {"xyz": ATTACH_XYZ, "rpy": ATTACH_RPY})
for e in links + joints:
    arm.append(e)
# fused small-board base (2026-07-11) replaces the old detachable base visual
for l in arm.findall("link"):
    if l.get("name") == "servo_0":
        for v in l.findall("visual"):
            m = v.find("geometry/mesh")
            if m is not None and "base_holder" in m.get("filename"):
                m.set("filename", "base_fused_smallboard.stl")
ET.indent(ET.ElementTree(arm), "  ")
ET.ElementTree(arm).write(OUT, encoding="unicode", xml_declaration=True)
print(f"wrote {OUT} (gripper on {ATTACH_PARENT} @ {ATTACH_XYZ})")