"""Parametric modular arm from the LOCKED base parts (wrap_holder + yoke_exact). Each MODULE = servo+holder (parent) -> revolute output -> yoke carrying the NEXT module. The whole arm is f(holder, yoke, connector, chain): change the per-joint BENDS to trace smith300's rough shape; change the reach/parts to retarget. Frames (mm, our servo STEP frame): servo output axis = +Y through (-25.5,0). The yoke grips the output and reaches its next-servo seat at SERVO_OFF; subsequent joints sit at the next servo's own output (SERVO_OFF + OUT_X). """ import xml.etree.ElementTree as ET HOLDER, YOKE, SERVO = "wrap_holder.stl", "yoke_arm.stl", "st3215.stl" SCALE = "0.001 0.001 0.001" # --- connector geometry (parametric; derived from the yoke link length) --- ARM_YOKE_LEN = 45.0 # mm; compact inter-servo connector (locked yoke_exact = 70mm) OUT_X = -0.0255 # servo base -> its output axis (joint), m SERVO_OFF = -(34.7 + ARM_YOKE_LEN) / 1000.0 # output frame -> next servo base, m YOKE_OFF = -OUT_X # render the yoke back in the gripped servo's frame # --- chain: per-joint BEND (rpy rad) applied at each joint to trace smith300's shape --- # v4: planar bends (about Z) to make the chain rise from the base and curve over like # smith300 (base -> shoulder up -> elbow -> wrist), kept in one plane. N = 6 BENDS = [ (0, -1.5708, 0), # base: pitch the chain up to vertical (0, 0, 0), (0, -1.0472, 0), # shoulder: bend ~60 forward (0, 0, 0), (0, -1.0472, 0), # elbow/wrist: bend ~60 down (0, 0, 0), ] COLORS = {"servo": "0.32 0.32 0.36 1", "holder": "0.20 0.52 0.85 1", "yoke": "0.85 0.60 0.20 1"} # fiducial (ArUco) exoskeleton: an ArUco BOARD per link on the holder's +Z face, recessed # at a KNOWN pose. Faithful to medium_single_redo: base=3x3@50mm id0; shoulders=2x2@15mm. # DICT_4X4_250. Generator emits each board's link->board pose (the fidex LinkConfig). FIDEX = True FIDEX_TEX = False # if True, map the real ArUco images onto the board faces (MuJoCo render) ARUCO_BASE_M = 0.050 # base board (3x3) ARUCO_LINK_M = 0.015 # shoulder boards (2x2) FIDEX_CFG = [] # collected LinkConfig rows (the CAD<->vision coupling) def _box(link, xyz, size, rgba, name): v = ET.SubElement(link, "visual") ET.SubElement(v, "origin", {"xyz": xyz, "rpy": "0 0 0"}) ET.SubElement(ET.SubElement(v, "geometry"), "box", {"size": size}) ET.SubElement(ET.SubElement(v, "material", {"name": name + str(id(link))}), "color", {"rgba": rgba}) def _aruco(link, link_name, servo_xyz, board_m, grid, ids): """Fiducial exoskeleton board on the holder's +Z face: standoff + dark holder frame + ArUco board plate, at a KNOWN pose. Records the link->board pose for the fidex LinkConfig.""" if not FIDEX: return sx = float(servo_xyz.split()[0]) zf = 0.0158 # holder +Z face (m) board_z = zf + 0.0038 # board plane (seated in the holder pocket), m # printed marker holder bracket (recessed board pocket) mounted on the +Z face hstl = "fidex_holder_bg.stl" if board_m > 0.03 else "fidex_holder_sm.stl" v = ET.SubElement(link, "visual") ET.SubElement(v, "origin", {"xyz": f"{sx} 0 {zf}", "rpy": "0 0 0"}) ET.SubElement(ET.SubElement(v, "geometry"), "mesh", {"filename": hstl, "scale": SCALE}) ET.SubElement(ET.SubElement(v, "material", {"name": "fxh" + str(id(link))}), "color", {"rgba": "0.16 0.16 0.18 1"}) # ArUco board seated in the pocket (real texture in FIDEX_TEX mode, else white plate) if FIDEX_TEX: v2 = ET.SubElement(link, "visual") ET.SubElement(v2, "origin", {"xyz": f"{sx} 0 {board_z}", "rpy": "0 0 0"}) ET.SubElement(ET.SubElement(v2, "geometry"), "box", {"size": f"{board_m:.4f} {board_m:.4f} 0.0016"}) mat = ET.SubElement(v2, "material", {"name": "mk_" + link_name}) ET.SubElement(mat, "texture", {"filename": f"fidex_markers/{link_name}.png"}) else: _box(link, f"{sx} 0 {board_z}", f"{board_m:.4f} {board_m:.4f} 0.0016", "0.96 0.96 0.96 1", "fxmark") # LinkConfig-compatible row (feeds Cameron's fidex pipeline directly) FIDEX_CFG.append({ "link": link_name, "mujoco_name": link_name, "robot_mesh_path": HOLDER, # printed holder on this link "exo_mesh_path": hstl, # the marker bracket "aruco_offset_pos": [round(sx * 1000, 2), 0.0, round(board_z * 1000, 2)], # mm, link->board "aruco_offset_rot": [0.0, 0.0, 0.0], # euler rad "aruco_board_name": f"board_{link_name}", "board_length": round(board_m, 4), # m "grid": grid, "ids": ids, "dict": "DICT_4X4_250", "marker_image": f"fidex_markers/{link_name}.png", }) def _vis(link, fname, cname, xyz): v = ET.SubElement(link, "visual") ET.SubElement(v, "origin", {"xyz": xyz, "rpy": "0 0 0"}) ET.SubElement(ET.SubElement(v, "geometry"), "mesh", {"filename": fname, "scale": SCALE}) ET.SubElement(ET.SubElement(v, "material", {"name": cname + str(id(link))}), "color", {"rgba": COLORS[cname]}) # orient the whole arm so the base plate sits down (base-on-table look). Tune. ROOT_RPY = (3.14159, 0, 0) def gen_urdf(): robot = ET.Element("robot", {"name": "smith300_modular"}) ET.SubElement(robot, "link", {"name": "world"}) bj = ET.SubElement(robot, "joint", {"name": "base_fix", "type": "fixed"}) ET.SubElement(bj, "parent", {"link": "world"}) ET.SubElement(bj, "child", {"link": "L0"}) ET.SubElement(bj, "origin", {"xyz": "0 0 0", "rpy": f"{ROOT_RPY[0]} {ROOT_RPY[1]} {ROOT_RPY[2]}"}) # L0 = first servo + holder (servo base at link origin) L0 = ET.SubElement(robot, "link", {"name": "L0"}) _box(L0, "-0.01 0 -0.024", "0.075 0.075 0.006", "0.90 0.75 0.15 1", "baseplate") # table mount _vis(L0, SERVO, "servo", "0 0 0") _vis(L0, HOLDER, "holder", "0 0 0") _aruco(L0, "L0", "0 0 0", ARUCO_BASE_M, [3, 3], list(range(9))) # base board 3x3, ids 0-8 prev = "L0" for i in range(1, N + 1): # joint sits at the PREVIOUS servo's output axis (+Y). For L0 that's OUT_X; for a # carried module the previous servo base is at SERVO_OFF, so its output is SERVO_OFF+OUT_X. jx = OUT_X if i == 1 else (SERVO_OFF + OUT_X) br, bp, by = BENDS[i - 1] j = ET.SubElement(robot, "joint", {"name": f"j{i}", "type": "revolute"}) ET.SubElement(j, "parent", {"link": prev}) ET.SubElement(j, "child", {"link": f"L{i}"}) ET.SubElement(j, "origin", {"xyz": f"{jx} 0 0", "rpy": f"{br} {bp} {by}"}) ET.SubElement(j, "axis", {"xyz": "0 1 0"}) ET.SubElement(j, "limit", {"lower": "-1.74", "upper": "1.74", "effort": "2.94", "velocity": "3.0"}) Li = ET.SubElement(robot, "link", {"name": f"L{i}"}) # yoke grips the previous servo's output (render back in that servo's frame) _vis(Li, YOKE, "yoke", f"{YOKE_OFF} 0 0") # next servo + holder seated at the yoke reach _vis(Li, SERVO, "servo", f"{SERVO_OFF} 0 0") _vis(Li, HOLDER, "holder", f"{SERVO_OFF} 0 0") base_id = 20 + (i - 1) * 4 # shoulders: 2x2, ids 20,24,28.. _aruco(Li, f"L{i}", f"{SERVO_OFF} 0 0", ARUCO_LINK_M, [2, 2], [base_id + k for k in range(4)]) prev = f"L{i}" # parametric ACTUATED 2-finger gripper fixed at the last servo's output gx = SERVO_OFF + OUT_X gj = ET.SubElement(robot, "joint", {"name": "gripper_mount", "type": "fixed"}) ET.SubElement(gj, "parent", {"link": prev}) ET.SubElement(gj, "child", {"link": "gripper"}) ET.SubElement(gj, "origin", {"xyz": f"{gx} 0 0", "rpy": "0 0 0"}) gl = ET.SubElement(robot, "link", {"name": "gripper"}) _box(gl, "-0.012 0 0", "0.022 0.038 0.024", "0.85 0.60 0.20 1", "gpalm") # palm # two prismatic fingers: q=0 open (+-12mm), q=0.012 closed (together) for fname, z0 in [("finger_l", 0.018), ("finger_r", -0.018)]: fj = ET.SubElement(robot, "joint", {"name": fname, "type": "prismatic"}) ET.SubElement(fj, "parent", {"link": "gripper"}) ET.SubElement(fj, "child", {"link": fname}) ET.SubElement(fj, "origin", {"xyz": f"-0.026 0 {z0}", "rpy": "0 0 0"}) ET.SubElement(fj, "axis", {"xyz": f"0 0 {-1 if z0 > 0 else 1}"}) # slide toward centre ET.SubElement(fj, "limit", {"lower": "0", "upper": "0.018", "effort": "5", "velocity": "0.1"}) fl = ET.SubElement(robot, "link", {"name": fname}) _box(fl, "-0.018 0 0", "0.036 0.014 0.006", "0.30 0.30 0.34 1", "fp_" + fname) # jaw return robot if __name__ == "__main__": import json import sys tex = "tex" in sys.argv if tex: FIDEX_TEX = True out = "smith300_modular_tex.urdf" if tex else "smith300_modular.urdf" tree = ET.ElementTree(gen_urdf()) ET.indent(tree, " ") tree.write(out, encoding="unicode", xml_declaration=True) if not tex: with open("fidex_config.json", "w") as f: json.dump(FIDEX_CFG, f, indent=2) print(f"wrote {out} (N={N} modules) + {len(FIDEX_CFG)} boards tex={tex}")