"""DOUBLE-SERVO joint module (Cameron 2026-07-13): two STS3215 coaxial, servo B flipped 180deg so both
horns face the shared GAP; each wears a standard thin yoke; yokes joined by a spacer across the gap into
ONE output part. Holders joined on the body side. Gap = 28mm (stubby-driver access to both horn plates).
Outputs: double_yoke.stl, double_holder.stl, double_joint.urdf. NOTE for calibration: servo B runs
direction-REVERSED (mirrored mounting)."""
import numpy as np, trimesh
import xml.etree.ElementTree as ET

GAP = 28.0
HORN_A = 9.6
c = (HORN_A + HORN_A + GAP) / 2                     # flip center: B horn lands at HORN_A+GAP
Rx = trimesh.transformations.rotation_matrix(np.pi, [1, 0, 0], point=[0, c, 0])
def U(p): return trimesh.boolean.union(p, engine="manifold")
def box(x0, x1, y0, y1, z0, z1):
    b = trimesh.creation.box(extents=[x1-x0, y1-y0, z1-z0])
    b.apply_translation([(x0+x1)/2, (y0+y1)/2, (z0+z1)/2]); return b

yA = trimesh.load("yoke_gen_thin.stl", force="mesh")
yB = yA.copy(); yB.apply_transform(Rx)
# spacer joins the two bridge walls (x -53..-47) across the gap; plates' outer faces at 11.6 / GAP+9.6-2
spacer = box(-53.0, -47.0, 11.6 - 0.4, HORN_A + GAP - 2.0 + 0.4, -13.0, 13.0)
dyoke = U([yA, yB, spacer])
dyoke.export("double_yoke.stl")
print(f"double_yoke: wt={dyoke.is_watertight} bodies={len(dyoke.split(only_watertight=False))} vol={dyoke.volume/1000:.1f}cm3")
hA = trimesh.load("holder_fillet.stl", force="mesh")
hB = hA.copy(); hB.apply_transform(Rx)
hbridge = box(8.0, 18.8, HORN_A - 0.4, HORN_A + GAP + 0.4, -9.0, 9.0)
dholder = U([hA, hB, hbridge])
dholder.export("double_holder.stl")
print(f"double_holder: wt={dholder.is_watertight} bodies={len(dholder.split(only_watertight=False))} vol={dholder.volume/1000:.1f}cm3")
# clearance: holder bridge vs swept yoke (revolve yoke about the axis x=-25.5,z=0 -> max radius check)
V = dyoke.vertices
r = np.sqrt((V[:, 0] + 25.5) ** 2 + V[:, 2] ** 2)
mask = (V[:, 1] > HORN_A - 1) & (V[:, 1] < HORN_A + GAP + 1)
r_yoke = r[mask].max()
bx, bz = 8.0, 9.0
r_bridge = np.sqrt((bx + 25.5) ** 2 + bz ** 2)
print(f"yoke sweep radius in gap: {r_yoke:.1f}mm | holder bridge inner radius: {r_bridge:.1f}mm | clearance {r_bridge - r_yoke:.1f}mm")
# servo B flipped mesh for viz
sB = trimesh.load("st3215.stl", force="mesh"); sB.apply_transform(Rx); sB.export("gnode_servoB_flipped.stl")
R = ET.Element("robot", {"name": "double_joint"}); 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, "gnode_servoB_flipped.stl", "0.45 0.40 0.35 1")
vis(base, "double_holder.stl", "0.20 0.52 0.85 1")
jj = ET.SubElement(R, "joint", {"name": "joint", "type": "revolute"})
ET.SubElement(jj, "parent", {"link": "base"}); ET.SubElement(jj, "child", {"link": "out"})
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": "-1.57", "upper": "1.57", "effort": "6", "velocity": "3"})
out = ET.SubElement(R, "link", {"name": "out"})
vis(out, "double_yoke.stl", "0.30 0.70 0.40 1", off="0.0255 0 0")
t = ET.ElementTree(R); ET.indent(t, "  ")
t.write("double_joint.urdf", encoding="unicode", xml_declaration=True)
print("wrote double_joint.urdf")