""" Parametric MODULAR robot generator — approximately resembles smith300, but built only from a CONSTANT reusable holder + GENERATED connectors (+ the custom gripper). Per arm link: - sts3215 servo (pose read from smith300.urdf; the real servo mesh, meters) - reusable holder (ours_base_holder.stl) at servo_pose @ holder_on_servo [CONSTANT] - generated connector (build123d) spanning the link's input joint -> its output joint Gripper links (eef_base/eef_thumb) keep their custom meshes. Kinematics (joint directions + orientations) come from smith300.urdf (fixed); the LINK LENGTHS (joint-translation magnitudes) are the free parameters and rescale both the joint positions, the servo/holder placement, and the connector lengths together. Run with the build123d venv (generates connector STLs + the URDF): .venv_cad/bin/python param_robot.py [scale] """ import sys, math, os import xml.etree.ElementTree as ET import numpy as np import build123d as bd from connector_part import make_connector URDF_IN = "/data/cameron/repos/smith300_para_stuff/smith300.urdf" OUT_DIR = "/data/cameron/repos/smith300_para_stuff" CONN_DIR = os.path.join(OUT_DIR, "gen_connectors") # constant module transforms (MJCF link frame), from generate_example_twolink_custom.py HOLDER_POS, HOLDER_QUAT = [-0.006365, 0, -0.0024], [1, 1, 1, 1] SERVO_POS, SERVO_QUAT = [0.0263353, 0, 0.0437], [1, 0, 0, 0] SHOULDER_POS, SHOULDER_QUAT = [0.0388353, 0, 0.0624], [0, 0, -1, 0] HOLDER_MESH = "assets/ours_base_holder.stl" # mm SERVO_MESH = "assets/sts3215_03a_v1.stl" # meters CONN_SECTION = (30.0, 16.0) # connector cross-section mm # ---------- SE3 ---------- def quatR(q): q = np.asarray(q, float); q = q / max(np.linalg.norm(q), 1e-12) w, x, y, z = q return np.array([[1-2*(y*y+z*z), 2*(x*y-w*z), 2*(x*z+w*y)], [2*(x*y+w*z), 1-2*(x*x+z*z), 2*(y*z-w*x)], [2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x*x+y*y)]]) def rpyR(r, p, y): cr, sr, cp, sp, cy, sy = math.cos(r), math.sin(r), math.cos(p), math.sin(p), math.cos(y), math.sin(y) return (np.array([[cy,-sy,0],[sy,cy,0],[0,0,1]]) @ np.array([[cp,0,sp],[0,1,0],[-sp,0,cp]]) @ np.array([[1,0,0],[0,cr,-sr],[0,sr,cr]])) def se3(R, t): T = np.eye(4); T[:3, :3] = R; T[:3, 3] = t; return T def R_to_rpy(R): p = math.atan2(-R[2, 0], math.hypot(R[0, 0], R[1, 0])) if abs(math.cos(p)) < 1e-8: return 0.0, p, math.atan2(-R[0, 1], R[1, 1]) return math.atan2(R[2, 1], R[2, 2]), p, math.atan2(R[1, 0], R[0, 0]) def origin_of(T): r, p, y = R_to_rpy(T[:3, :3]); t = T[:3, 3] return {"xyz": f"{t[0]:.6g} {t[1]:.6g} {t[2]:.6g}", "rpy": f"{r:.6g} {p:.6g} {y:.6g}"} T_HOLDER = se3(quatR(HOLDER_QUAT), HOLDER_POS) T_SERVO = se3(quatR(SERVO_QUAT), SERVO_POS) HOLDER_ON_SERVO = np.linalg.inv(T_SERVO) @ T_HOLDER def vec(s): return np.array([float(x) for x in s.split()], float) # ---------- parse smith300.urdf ---------- def parse(): root = ET.parse(URDF_IN).getroot() links = {} for link in root.findall("link"): name = link.get("name") info = {"servo": None, "eef": []} for v in link.findall("visual"): mesh = v.find("geometry/mesh") if mesh is None: continue fn = mesh.get("filename", "") o = v.find("origin") T = se3(rpyR(*vec(o.get("rpy", "0 0 0"))), vec(o.get("xyz", "0 0 0"))) if "sts3215" in fn: info["servo"] = T elif "eef" in fn: info["eef"].append((fn, mesh.get("scale", "1 1 1"), T)) links[name] = info joints = [] for j in root.findall("joint"): o = j.find("origin") xyz = vec(o.get("xyz")); rpy = vec(o.get("rpy")) joints.append({"name": j.get("name"), "parent": j.find("parent").get("link"), "child": j.find("child").get("link"), "xyz": xyz, "rpy": rpy, "axis": j.find("axis").get("xyz"), "limit": j.find("limit")}) order = [l.get("name") for l in root.findall("link")] return order, links, joints def align_R(d): """rotation mapping local +Z onto unit direction d.""" d = np.asarray(d, float); d = d / max(np.linalg.norm(d), 1e-12) z = np.array([0, 0, 1.0]); v = np.cross(z, d); c = float(z @ d) if np.linalg.norm(v) < 1e-9: return np.eye(3) if c > 0 else np.diag([1.0, -1.0, -1.0]) vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) return np.eye(3) + vx + vx @ vx * (1.0 / (1.0 + c)) def gen_connector(v_m, path): """Generate a mature connector (build123d) of the right length; canonical along +Z.""" L = float(np.linalg.norm(v_m)) * 1000.0 if L < 2.0: return None bd.export_stl(make_connector(L), path, tolerance=0.25, ascii_format=False) return L def mesh_visual(link_el, filename, scale, T, rgba=None): v = ET.SubElement(link_el, "visual") ET.SubElement(v, "origin", origin_of(T)) g = ET.SubElement(v, "geometry") ET.SubElement(g, "mesh", {"filename": filename, "scale": scale}) if rgba: m = ET.SubElement(v, "material", {"name": rgba[0]}) ET.SubElement(m, "color", {"rgba": rgba[1]}) def build(scale=1.0): order, links, joints = parse() os.makedirs(CONN_DIR, exist_ok=True) gripper_links = {name for name, i in links.items() if i["eef"]} # each arm link's OUTPUT joint (the joint it drives) — used to anchor the servo so it # tracks the (scaled) joint position instead of staying at a fixed link-local pose. outjoint = {jt["parent"]: jt for jt in joints} robot = ET.Element("robot", {"name": "smith300_modular"}) link_els = {name: ET.SubElement(robot, "link", {"name": name}) for name in order} # per-link geometry: servo + reusable holder (arm) OR eef mesh (gripper) for name in order: info = links[name] if name in gripper_links: for fn, sc, T in info["eef"]: mesh_visual(link_els[name], fn, sc, T, ("eef_mat", "0.85 0.2 0.2 1")) if info["servo"] is not None: mesh_visual(link_els[name], SERVO_MESH, "1 1 1", info["servo"], ("servo_mat", "0.15 0.15 0.17 1")) continue if info["servo"] is not None: # anchor servo to its output joint, scaled: servo follows the joint it drives jt = outjoint[name] T_out0 = se3(rpyR(*jt["rpy"]), jt["xyz"]) T_outs = se3(rpyR(*jt["rpy"]), jt["xyz"] * scale) T_servo = T_outs @ (np.linalg.inv(T_out0) @ info["servo"]) mesh_visual(link_els[name], SERVO_MESH, "1 1 1", T_servo, ("servo_mat", "0.15 0.15 0.17 1")) mesh_visual(link_els[name], HOLDER_MESH, "0.001 0.001 0.001", T_servo @ HOLDER_ON_SERVO, ("holder_mat", "0.2 0.45 0.85 1")) # joints (scaled translations) + generated connectors on the parent arm link for jt in joints: v = jt["xyz"] * scale j = ET.SubElement(robot, "joint", {"name": jt["name"], "type": "revolute"}) ET.SubElement(j, "parent", {"link": jt["parent"]}) ET.SubElement(j, "child", {"link": jt["child"]}) ET.SubElement(j, "origin", {"xyz": f"{v[0]:.6g} {v[1]:.6g} {v[2]:.6g}", "rpy": f"{jt['rpy'][0]:.6g} {jt['rpy'][1]:.6g} {jt['rpy'][2]:.6g}"}) ET.SubElement(j, "axis", {"xyz": jt["axis"]}) if jt["limit"] is not None: ET.SubElement(j, "limit", dict(jt["limit"].attrib)) # mature connector on the PARENT link, oriented from parent-origin -> this joint if jt["parent"] not in gripper_links and np.linalg.norm(v) > 1e-6: stl = os.path.join(CONN_DIR, f"conn_{jt['name']}.stl") if gen_connector(v, stl): T_conn = se3(align_R(v / np.linalg.norm(v)), [0, 0, 0]) mesh_visual(link_els[jt["parent"]], os.path.relpath(stl, OUT_DIR), "0.001 0.001 0.001", T_conn, ("conn_mat", "0.85 0.72 0.3 1")) return robot def main(): scale = float(sys.argv[1]) if len(sys.argv) > 1 else 1.0 robot = build(scale) out = os.path.join(OUT_DIR, "smith300_modular.urdf" if scale == 1.0 else f"smith300_modular_x{scale}.urdf") ET.ElementTree(robot).write(out, encoding="utf-8", xml_declaration=True) print(f"wrote {out} links={len(robot.findall('link'))} joints={len(robot.findall('joint'))} scale={scale}") if __name__ == "__main__": main()