"""L-post hanging-arm sketch (Cameron 2026-07-14): 2040 vertical post + 2040 top rail + 2020 diagonal
gusset, arm hung INVERTED from a printed rail plate, spring anchored ON the yaw axis (yaw-invariant),
fiducial board on the foot plate. Sketch quality: spring drawn at plumb pose. -> lpost_arm.urdf"""
import numpy as np, trimesh
import xml.etree.ElementTree as ET

def box(e, p, rot=None):
    b = trimesh.creation.box(extents=e)
    if rot is not None: b.apply_transform(rot)
    b.apply_translation(p); return b
POST_H, RAIL_L, HANG_X = 700.0, 450.0, 30.0
parts = [
    box([40, 20, POST_H], [-350, 0, POST_H/2]),                                  # 2040 post
    box([RAIL_L, 20, 40], [-350 + RAIL_L/2 - 20, 0, POST_H + 20]),               # 2040 top rail
    box([200, 140, 8], [-290, 0, 4]),                                            # foot plate
    box([60, 60, 8], [HANG_X, 0, POST_H - 4]),                                   # printed rail-mount plate
]
g = trimesh.creation.box(extents=[20, 20, 280])
g.apply_transform(trimesh.transformations.rotation_matrix(np.pi/4, [0, 1, 0]))
g.apply_translation([-255, 0, POST_H - 105])
parts.append(g)                                                                   # 2020 diagonal gusset
frame = trimesh.util.concatenate(parts)
frame.export("gnode_lpost_frame.stl")
board = box([80, 80, 4], [-180, 0, 10])
board.export("gnode_lpost_board.stl")
eye = trimesh.creation.torus(major_radius=6, minor_radius=2.2)
eye.apply_transform(trimesh.transformations.rotation_matrix(np.pi/2, [1, 0, 0]))
eye.apply_translation([HANG_X - 25.5, 0, POST_H - 12])
eye.export("gnode_lpost_anchor.stl")                                              # anchor ON the yaw axis
spring = trimesh.creation.cylinder(radius=3.5, height=240, sections=20)
spring.apply_translation([HANG_X - 25.5, 0, POST_H - 20 - 120])
spring.export("gnode_lpost_spring.stl")
# arm re-rooted: hang inverted from the plate
t = ET.parse("two_servo_beauty.urdf"); r = t.getroot(); r.set("name", "lpost_arm")
wj = next(j for j in r.findall("joint") if j.get("name") == "world_to_servo_0")
wj.find("origin").set("xyz", f"{HANG_X/1000:.4f} 0 {(POST_H - 8)/1000:.4f}")
wj.find("origin").set("rpy", "-1.5708 0 0")
world = next(l for l in r.findall("link") if l.get("name") == "world")
def vis(link, fn, rgba):
    v = ET.SubElement(link, "visual"); ET.SubElement(v, "origin", {"xyz": "0 0 0", "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})
vis(world, "gnode_lpost_frame.stl", "0.55 0.57 0.62 1")
vis(world, "gnode_lpost_board.stl", "0.10 0.10 0.10 1")
vis(world, "gnode_lpost_anchor.stl", "0.85 0.35 0.25 1")
vis(world, "gnode_lpost_spring.stl", "0.75 0.75 0.30 1")
# swap base visual to the plain holder (no table board on the hanging base)
for l in r.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" in (m.get("filename") or ""):
                m.set("filename", "base_holder.stl")
ET.indent(t, "  "); t.write("lpost_arm.urdf", encoding="unicode", xml_declaration=True)
print("wrote lpost_arm.urdf")