"""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. """ 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 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) if i > 0 and os.path.exists(CONNECTOR_FMT.format(i)): # connector rides in servo_i's local frame c = trimesh.load(CONNECTOR_FMT.format(i), force="mesh") 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}", transform=np.eye(4)) if src_glb: # base pieces ride with servo_0 (still editable + round-trips) src = trimesh.load(src_glb) src_base_inv = np.linalg.inv(np.array(src.graph["servo_0"][0], float)) for node in src.graph.nodes_geometry: if node.startswith("base_cube"): T, gname = src.graph[node] scene.add_geometry(src.geometry[gname].copy(), node_name=node, geom_name=node, parent_node_name="servo_0", # rebase to servo_0 so no shift offset transform=src_base_inv @ np.array(T, float)) scene.export(path) print("wrote", path, "nodes:", sorted(scene.graph.nodes_geometry), f"scale={GLB_SCALE}x") def read_layout(path, scale=GLB_SCALE): """Read servo_i node transforms from a (scaled) GLB Cameron exported -> LAYOUT in metres (servo_0 frame). Divides translation by `scale`; rotation is scale-invariant. Rebases to servo_0 = identity.""" scene = trimesh.load(path) poses = {} for node in scene.graph.nodes_geometry: T, _ = scene.graph[node] T = np.array(T, float).copy() T[:3, 3] /= scale poses[node] = T def _key(node): # order: servo_0, servo_1.001, .002, .003, .004, head, _, tail = node.partition(".") # then bare servo_1 LAST (it's Cameron's 6th servo) return (int(head.split("_")[1]), int(tail) if tail else 10**6) order = sorted((k for k in poses if k.startswith("servo_")), key=_key) base_inv = np.linalg.inv(poses["servo_0"]) # express everything in servo_0's frame return [base_inv @ poses[n] for n in order] YOKE_FACE_PAD = 6.0 # mm: expand the (thin 16x22) yoke arm-end mating face so the connector base is # fuller + more secure at the yoke end (0 = raw arm cross-section) CONNECT_MODE = "closest" # "closest" = mate the CLOSEST pair between the yoke-arm box faces and the holder # box faces (generalizes to any orientation, e.g. top-of-yoke->bottom-of-holder); # "fixed" = the original yoke arm-END face -> holder +X face YOKE_ARM_X = -48.0 # arm/spine boundary in the yoke frame (yoke_exact: SPINE_X - SPINE_T/2) 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)) print(f"wrote connector_{i}.stl vol={c.volume/1000:.1f}cm3 watertight={c.is_watertight}") gen_urdf("two_servo.urdf") gen_glb("two_servo.glb")