import xml.etree.ElementTree as ET, sys, mujoco, numpy as np
xin, xout = sys.argv[1], sys.argv[2]
# servo_0 world rotation in the MJCF = Rz(180)*Rx(90) (flip + Y-up->Z-up); +Y (plate top) -> world +Z
R = np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]], float)
lift = 0.0
for j in ET.parse("two_servo.urdf").getroot().findall("joint"):
    if j.get("name") == "world_to_servo_0":
        lift = float(j.find("origin").get("xyz").split()[2])
plate_local = np.array([-0.0221, 0.0044, -0.180])              # plate top face in servo_0 frame (m)
pos = R @ plate_local + np.array([0, 0, lift])
quat = np.zeros(4); mujoco.mju_mat2Quat(quat, R.flatten())
t = ET.parse(xin); r = t.getroot()
a = r.find("asset") or ET.SubElement(r, "asset")
ET.SubElement(a, "texture", {"name": "aruco", "type": "2d", "file": "assets/aruco_base_board.png"})
ET.SubElement(a, "material", {"name": "aruco", "texture": "aruco"})
ET.SubElement(r.find("worldbody"), "geom", {"type": "box", "size": "0.079 0.0004 0.079",
    "pos": f"{pos[0]:.4f} {pos[1]:.4f} {pos[2]:.4f}",
    "quat": f"{quat[0]:.5f} {quat[1]:.5f} {quat[2]:.5f} {quat[3]:.5f}",
    "material": "aruco", "contype": "0", "conaffinity": "0"})
t.write(xout)
m = mujoco.MjModel.from_xml_path(xout); print("fidex OK nbody", m.nbody, "board_pos", np.round(pos, 3).tolist())