"""Two-servo layout -> URDF (canonical) + GLB (for Blender), from ONE shared LAYOUT list. This is the seed of the Blender<->connector pipeline: LAYOUT = module poses in servo_0's frame (meters, servo_0 = identity = base). Each module = servo + holder + yoke (constant, the parts we made). The GLB has one node per servo (servo_0, servo_1, ...) so Cameron can grab + reorient each in Blender and export it back; I then read those node transforms straight back into LAYOUT. No connector geometry yet. RECOVERY NOTE (2026-07-06): reconstructed after the phe108 outage. Most is verbatim from session context; sections marked [RECONSTRUCTED] (gen_glb tail, read_layout, two constants) were rewritten from documented behavior — diff against the lab original when it's back. """ import os import numpy as np import trimesh import xml.etree.ElementTree as ET from scipy.spatial.transform import Rotation as Rot from shapely.geometry import box as _sbox # module parts (mm, authored in the servo frame): file, urdf-color, glb face-color PARTS = [ ("st3215.stl", "0.35 0.35 0.40 1", [90, 90, 100, 255]), ("holder_approx_drilled.stl", "0.20 0.52 0.85 1", [52, 133, 217, 255]), ("yoke_exact.stl", "0.85 0.60 0.20 1", [217, 153, 51, 255]), ] def _T(xyz, rpy=(0, 0, 0)): M = np.eye(4) M[:3, :3] = Rot.from_euler("xyz", rpy).as_matrix() M[:3, 3] = xyz return M # LAYOUT: module pose in servo_0 frame (meters). servo_0 = base = identity. servo_1 ~150mm back along -X # (its holder faces servo_0's yoke) — a reasonable seed; Cameron will reorient in Blender. LAYOUT = [_T((0, 0, 0)), _T((-0.150, 0, 0))] def _vis(link, fn, col): off = "0.0255 0 0" if fn.startswith("yoke") else "0 0 0" # yoke rendered back into the servo frame 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": fn + str(id(v))}), "color", {"rgba": col}) OUT = _T((-0.0255, 0, 0)) # servo output-joint transform in the servo frame (+Y axis) VIEW_RPY = "1.5708 0 0" # root display rot: model is glTF Y-up; +90 deg X makes it Z-up like Blender/viewer def _origin(M): r, p, y = Rot.from_matrix(M[:3, :3]).as_euler("xyz") t = M[:3, 3] return {"xyz": f"{t[0]:.6f} {t[1]:.6f} {t[2]:.6f}", "rpy": f"{r:.6f} {p:.6f} {y:.6f}"} def _assembly_min_y(): """Lowest servo_0-frame Y over the whole assembly (modules + connectors + base). After VIEW_RPY (+90 X) world-Z == servo_0-frame Y, so this is the robot's lowest point; -this lifts it onto the floor (Z=0).""" ys = [] mod = module_mesh() for i, M in enumerate(LAYOUT): ys.append(float(trimesh.transform_points(mod.vertices, _mm(M))[:, 1].min())) if i > 0 and os.path.exists(CONNECTOR_FMT.format(i)): c = trimesh.load(CONNECTOR_FMT.format(i), force="mesh") ys.append(float(trimesh.transform_points(c.vertices, _mm(M))[:, 1].min())) if os.path.exists("base_holder.stl"): ys.append(float(trimesh.load("base_holder.stl", force="mesh").vertices[:, 1].min())) return min(ys) / 1000.0 # mm -> m def gen_urdf(path): """Proper serial chain: servo_0 -> output_0 -> yoke_0 -> connect_1 -> servo_1 -> output_1 -> yoke_1. servo_i is CARRIED BY servo_{i-1}'s yoke, so rotating output_{i-1} swings the whole downstream arm.""" robot = ET.Element("robot", {"name": "two_servo"}) ET.SubElement(robot, "link", {"name": "world"}) # display root wj = ET.SubElement(robot, "joint", {"name": "world_to_servo_0", "type": "fixed"}) ET.SubElement(wj, "parent", {"link": "world"}) ET.SubElement(wj, "child", {"link": "servo_0"}) lift = -_assembly_min_y() # sit the lowest point on the floor (Z=0) ET.SubElement(wj, "origin", {"xyz": f"0 0 {lift:.6f}", "rpy": VIEW_RPY}) # Y-up -> Z-up + ground the base for i in range(len(LAYOUT)): base = ET.SubElement(robot, "link", {"name": f"servo_{i}"}) # servo + holder _vis(base, PARTS[0][0], PARTS[0][1]) # servo body if i == 0 and os.path.exists("base_holder.stl"): # base servo: holder+cube unioned _vis(base, "base_holder.stl", PARTS[1][1]) else: _vis(base, PARTS[1][0], PARTS[1][1]) # regular holder if i > 0: # procedural connector _vis(base, CONNECTOR_FMT.format(i), "0.30 0.66 0.45 1") oj = ET.SubElement(robot, "joint", {"name": f"output_{i}", "type": "revolute"}) ET.SubElement(oj, "parent", {"link": f"servo_{i}"}) ET.SubElement(oj, "child", {"link": f"yoke_{i}"}) ET.SubElement(oj, "origin", {"xyz": "-0.0255 0 0", "rpy": "0 0 0"}) ET.SubElement(oj, "axis", {"xyz": "0 1 0"}) ET.SubElement(oj, "limit", {"lower": "-1.74", "upper": "1.74", "effort": "2.94", "velocity": "3.0"}) yk = ET.SubElement(robot, "link", {"name": f"yoke_{i}"}) _vis(yk, PARTS[2][0], PARTS[2][1]) if i > 0: # servo_i hangs off servo_{i-1}'s yoke. Fixed transform yoke_{i-1} -> servo_i (the future # connector lives here) = inv(OUT) @ inv(LAYOUT[i-1]) @ LAYOUT[i]. C = np.linalg.inv(OUT) @ np.linalg.inv(LAYOUT[i - 1]) @ LAYOUT[i] j = ET.SubElement(robot, "joint", {"name": f"connect_{i}", "type": "fixed"}) ET.SubElement(j, "parent", {"link": f"yoke_{i-1}"}) ET.SubElement(j, "child", {"link": f"servo_{i}"}) ET.SubElement(j, "origin", _origin(C)) ET.indent(ET.ElementTree(robot), " ") ET.ElementTree(robot).write(path, encoding="unicode", xml_declaration=True) print("wrote", path) def module_mesh(holder_override=None): """servo+holder+yoke concatenated (mm), face-coloured per part. holder_override swaps the holder file (e.g. base_holder.stl for servo_0) so the base pieces show and the plain holder isn't doubled.""" parts = [] for fn, _, fc in PARTS: f = holder_override if (holder_override and fn == PARTS[1][0]) else fn m = trimesh.load(f, force="mesh").copy() m.visual.face_colors = fc parts.append(m) return trimesh.util.concatenate(parts) GLB_SCALE = 10.0 # blow the GLB up so it's not tiny in Blender; divide back out on import (see read_layout) CONNECTOR_FMT = "connector_{}.stl" # variant hook (pretty version overrides this) CONNECTOR_ROUND = 7.0 # connector corner radius mm (0 = square); rounded is the default # [RECONSTRUCTED] constants used by connector_mesh (definitions lived in the lost span): YOKE_ARM_X = -48.0 # yoke verts with x < this = the arm/link region (arm starts past the head at -48) CONNECT_MODE = "closest" # "closest" = dynamic holder face (Cameron-approved) | "fixed" = legacy -X/+X def gen_glb(path, src_glb=None): """Export a PARENT-CHAIN GLB: servo_0 -> servo_1 -> ... -> servo_N, each with a transform relative to its parent. In Blender moving/rotating any servo then carries all its descendants. connector_i and the base pieces are parented to the servo they belong to so they ride along. Round-trip is unaffected (read_layout resolves world poses either way).""" scene = trimesh.Scene() base = module_mesh() # plain module; base pieces stay separate base.apply_scale(0.001 * GLB_SCALE) # mm -> m -> x GLB_SCALE for i, M in enumerate(LAYOUT): if i == 0: T = M.copy() T[:3, 3] *= GLB_SCALE scene.add_geometry(base.copy(), node_name="servo_0", geom_name="servo_0", transform=T) else: rel = np.linalg.inv(LAYOUT[i - 1]) @ LAYOUT[i] # servo_i relative to its parent servo_{i-1} rel[:3, 3] *= GLB_SCALE scene.add_geometry(base.copy(), node_name=f"servo_{i}", geom_name=f"servo_{i}", parent_node_name=f"servo_{i - 1}", transform=rel) # ---- [RECONSTRUCTED] connectors ride their servo ---- if i > 0 and os.path.exists(CONNECTOR_FMT.format(i)): c = trimesh.load(CONNECTOR_FMT.format(i), force="mesh").copy() c.visual.face_colors = [77, 168, 115, 255] c.apply_scale(0.001 * GLB_SCALE) scene.add_geometry(c, node_name=f"connector_{i}", geom_name=f"connector_{i}", parent_node_name=f"servo_{i}") # ---- [RECONSTRUCTED] base pieces pass-through: keep Cameron's base_cube* nodes editable, rebased so # they stay glued to servo_0 even after he moves it (base_inv = inv(src servo_0 world pose)) ---- if src_glb and os.path.exists(src_glb): src = trimesh.load(src_glb) src_base_inv = np.linalg.inv(np.array(src.graph["servo_0"][0], float)) for node in sorted(src.graph.nodes_geometry): if node.startswith("base_cube"): T, gn = src.graph[node] rel = src_base_inv @ np.array(T, float) # pose relative to servo_0 scene.add_geometry(src.geometry[gn].copy(), node_name=node, geom_name=node, parent_node_name="servo_0", transform=rel) scene.export(path) print("wrote", path) def read_layout(path): """[RECONSTRUCTED] Read Cameron's Blender GLB back into LAYOUT: resolve each servo_* node's WORLD pose through the parent chain, divide translations by GLB_SCALE (-> meters), order by index. Cameron's hacky Blender duplication makes nodes like servo_1.001 (poses live on the MESH nodes); sort key = (base_idx, sub_idx) with a BARE servo_N sorting LAST (his bare servo_1 was the end servo).""" scene = trimesh.load(path) entries = [] for node in scene.graph.nodes_geometry: if not node.startswith("servo"): continue name = node.split("/")[-1] head, _, tail = name.partition(".") try: base_idx = int(head.split("_")[1]) except (IndexError, ValueError): continue _key = (base_idx, int(tail) if tail else 10**6) W = np.array(scene.graph[node][0], float) # world transform (chain-resolved by trimesh) M = W.copy() M[:3, 3] /= GLB_SCALE # undo the display blow-up -> meters entries.append((_key, M)) entries.sort(key=lambda e: e[0]) lay = [M for _, M in entries] base_inv = np.linalg.inv(lay[0]) return [base_inv @ M for M in lay] # re-express in servo_0 frame (servo_0 = identity) def _box_faces(lo, hi): """The 6 faces of an axis-aligned box -> list of (center[3], corners[4,3], outward_normal[3]).""" faces = [] for ax in range(3): o = [a for a in range(3) if a != ax] for sgn, val in [(-1.0, lo[ax]), (1.0, hi[ax])]: corners = [[0, 0, 0] for _ in range(4)] for k, (s0, s1) in enumerate([(0, 0), (0, 1), (1, 1), (1, 0)]): corners[k][ax] = val corners[k][o[0]] = (lo[o[0]], hi[o[0]])[s0] corners[k][o[1]] = (lo[o[1]], hi[o[1]])[s1] center = [(lo[a] + hi[a]) / 2 for a in range(3)] center[ax] = val normal = [0.0, 0.0, 0.0] normal[ax] = sgn faces.append((np.array(center, float), np.array(corners, float), np.array(normal, float))) return faces def _closest_faces(yfaces, hfaces, Ty, Th): """Cameron 2026-07-01: keep the ORIGINAL yoke face + the current (dynamic) holder face. HOLDER face = the one facing the yoke (outward normal points most toward the yoke). YOKE face = the arm END face (-X, the original approved connector face). Returns (yoke_idx, holder_idx).""" yctr = trimesh.transform_points([np.mean([f[0] for f in yfaces], 0)], Ty)[0] hb = max(range(6), key=lambda b: (yctr - trimesh.transform_points([hfaces[b][0]], Th)[0]) @ (Th[:3, :3] @ hfaces[b][2])) # holder face most facing the yoke ya = next(a for a, (yc, ycorn, yn) in enumerate(yfaces) if yn[0] < -0.5) # arm END face (-X) return ya, hb def _pad_face(pts, pad): """Expand a (roughly planar) face point set outward within its plane by `pad` mm.""" pts = np.asarray(pts, float) c = pts.mean(0) nax = int(pts.var(0).argmin()) # face-normal axis (least spread) ext = pts.copy() for a in range(3): if a != nax: ext[:, a] += pad * np.sign(ext[:, a] - c[a]) return np.vstack([pts, ext]) def _mm(M): """Convert a metres transform to apply to a mm mesh (scale only the translation).""" M2 = np.array(M, float).copy() M2[:3, 3] *= 1000.0 return M2 def _pad_corners(corners, pad): """Expand a 4-corner rectangle outward within its own plane by `pad` mm (keeps 4 corners).""" corners = np.asarray(corners, float) c = corners.mean(0) nax = int(corners.var(0).argmin()) out = corners.copy() for a in range(3): if a != nax: out[:, a] += pad * np.sign(out[:, a] - c[a]) return out def _loft_faces(qa, qb): """SOLID frustum between two 4-corner rectangles (world). Corners are ordered CCW and matched by nearest so the sides don't cross; the result keeps a full rectangular cross-section and TWISTS to follow any relative rotation (no triangular hull collapse). Watertight.""" def ccw(q): q = np.asarray(q, float) c = q.mean(0) n = np.cross(q[1] - q[0], q[2] - q[0]) n = n / np.linalg.norm(n) u = (q[0] - c) / np.linalg.norm(q[0] - c) v = np.cross(n, u) return q[np.argsort([np.arctan2((p - c) @ v, (p - c) @ u) for p in q])] qa, qb = ccw(qa), ccw(qb) best = None for cand0 in (qb, qb[::-1]): # match qb->qa: min total corner dist for r in range(4): cand = np.roll(cand0, r, axis=0) d = float(np.sum(np.linalg.norm(qa - cand, axis=1))) if best is None or d < best[0]: best = (d, cand) qb = best[1] V = np.vstack([qa, qb]) F = [[0, 1, 2], [0, 2, 3], [4, 6, 5], [4, 7, 6]] # two end caps for i in range(4): j = (i + 1) % 4 F += [[i, j, 4 + j], [i, 4 + j, 4 + i]] # four side quads m = trimesh.Trimesh(V, np.array(F), process=False) m.fix_normals() return m def connector_mesh(i): """PERPENDICULAR BLOCK: take the holder's yoke-facing face and extrude it straight to the yoke as a UNIFORM rectangular prism (the holder face cross-section held constant) — a block dropping off the holder, not a slanted taper. The holder-sized bottom engulfs + bonds the yoke arm end.""" yoke = trimesh.load("yoke_exact.stl", force="mesh") holder = trimesh.load("holder_approx_drilled.stl", force="mesh") Ty, Th = _mm(LAYOUT[i - 1]), _mm(LAYOUT[i]) arm = yoke.vertices[yoke.vertices[:, 0] < YOKE_ARM_X] yfaces, hfaces = _box_faces(arm.min(0), arm.max(0)), _box_faces(*holder.bounds) if CONNECT_MODE == "closest": a, b = _closest_faces(yfaces, hfaces, Ty, Th) else: # fixed: arm-end (-X) -> holder +X a = next(k for k, f in enumerate(yfaces) if f[2][0] < -0.5) b = next(k for k, f in enumerate(hfaces) if f[2][0] > 0.5) hw = trimesh.transform_points(hfaces[b][1], Th) # holder face corners (world) hc = hw.mean(0) yc = trimesh.transform_points([yfaces[a][0]], Ty)[0] # yoke face centre (world) bdir = (yc - hc) / np.linalg.norm(yc - hc) # holder -> yoke direction arm_w = trimesh.transform_points(arm, Ty) # yoke arm in world depth = float((arm_w @ bdir).max() - hc @ bdir) + 4.0 # reach the FAR (bottom) of the arm + 4mm if CONNECTOR_ROUND > 0: # ROUNDED-rectangle cross-section (default) e1, e2 = hw[1] - hw[0], hw[3] - hw[0] h1, h2 = np.linalg.norm(e1) / 2, np.linalg.norm(e2) / 2 e1u, e2u = e1 / np.linalg.norm(e1), e2 / np.linalg.norm(e2) r = float(min(CONNECTOR_ROUND, h1 - 1.0, h2 - 1.0)) poly = _sbox(-h1 + r, -h2 + r, h1 - r, h2 - r).buffer(r, join_style=1, resolution=8) conn = trimesh.creation.extrude_polygon(poly, 1.0) # local prism z:0->1 Mm = np.eye(4) Mm[:3, 0], Mm[:3, 1], Mm[:3, 2], Mm[:3, 3] = e1u, e2u, bdir * depth, hc # local -> world conn.apply_transform(Mm) else: # square uniform prism (legacy) conn = _loft_faces(hw, hw + bdir * depth) conn.apply_transform(_mm(np.linalg.inv(LAYOUT[i]))) # world -> servo_i frame return conn if __name__ == "__main__": for i in range(1, len(LAYOUT)): c = connector_mesh(i) c.export(CONNECTOR_FMT.format(i))