"""SO-100 parametric rebuild from OUR locked parts (st3215 + wrap_holder + yoke_exact). PHASE 1: reproduce the SO-ARM100 kinematic chain EXACTLY (joint origins/rpy/axes/limits from Simulation/SO100/so100.urdf), but at each link place OUR servo+holder so the servo's real output axis (+Y through (-0.0255,0,0) m) coincides with the joint that link drives. This is physically correct (output shaft IS the joint) and needs no mesh registration — only the joint axis, which is identical between their sts3215 and our st3215. Now 6 DOF + gripper (a parametric wrist_yaw was added on top of the SO-100's 5) = 7 servos. Connectors come in Phase 2. Each link i carries the servo for its OUTGOING joint i; the body straddles the joint and extends back into the link. ROLLS[i] is the free roll about each output axis (optimized by so100_optimize_rolls.py to aim each yoke at the next servo). The chain is fully parametric: add/remove a JOINTS+LINKS entry (and a ROLLS/SPACERS slot) and the whole arm regenerates. """ import numpy as np import xml.etree.ElementTree as ET from scipy.spatial.transform import Rotation as Rot SERVO, HOLDER, YOKE = "st3215.stl", "wrap_holder.stl", "yoke_exact.stl" SCALE = "0.001 0.001 0.001" P0 = np.array([-0.0255, 0.0, 0.0]) # point on our output axis (m), output dir = +Y # SO-100 chain: (name, parent, child, xyz(m), rpy(rad), axis, lower, upper) JOINTS = [ ("shoulder_pan", "base", "shoulder", (0, -0.0452, 0.0165), (1.57079, 0, 0), (0, 1, 0), -2, 2), ("shoulder_lift", "shoulder", "upper_arm", (0, 0.1025, 0.0306), (-1.8, 0, 0), (1, 0, 0), 0, 3.5), ("elbow_flex", "upper_arm", "lower_arm", (0, 0.11257, 0.028), (1.57079, 0, 0), (1, 0, 0), -3.14158, 0), ("wrist_flex", "lower_arm", "wrist", (0, 0.0052, 0.1349), (-1, 0, 0), (1, 0, 0), -2.5, 1.2), ("wrist_roll", "wrist", "wrist2", (0, -0.0601, 0), (0, 1.57079, 0), (0, 1, 0), -3.14158, 3.14158), # 6th DOF (parametric add): perpendicular wrist YAW inserted before the gripper. Demonstrates the # generator is truly parametric — one JOINTS/LINKS entry + a SPACERS/ROLLS slot and the whole arm # (servo placement, optimized yokes, printable bridges, collision check) regenerates around it. ("wrist_yaw", "wrist2", "gripper", (0, -0.045, 0), (1.57079, 0, 0), (0, 1, 0), -1.2, 1.8), ("gripper", "gripper", "jaw", (-0.0202, -0.0244, 0),(0, 3.14158, 0), (0, 0, 1), -0.2, 2.0), ] LINKS = ["base", "shoulder", "upper_arm", "lower_arm", "wrist", "wrist2", "gripper", "jaw"] ROLLS = [1.3963, 4.8433, 3.0107, 1.8762, 4.8869, 0.2618, 4.407] # optimized for 6-DOF chain # SPACERS[i] = extra metres added along joint i's bridge so our (bigger) servos don't intersect their # neighbours (SO-100's own parts are smaller). Tuned via so100_check.py until 0 servo-servo overlaps. SPACERS = [0, 0, 0.055, 0.02, 0.05, 0.03, 0.07] def _spaced(xyz, s): x = np.array(xyz, float) n = np.linalg.norm(x) return tuple(x * (1 + s / n)) if n > 1e-9 else tuple(x) JOINTS = [(n, p, c, _spaced(xyz, SPACERS[i]), rpy, ax, lo, hi) for i, (n, p, c, xyz, rpy, ax, lo, hi) in enumerate(JOINTS)] COLORS = {"servo": "0.15 0.15 0.17 1", "holder": "0.20 0.52 0.85 1", "yoke": "0.85 0.60 0.20 1"} def _T(xyz, rpy): M = np.eye(4) M[:3, :3] = Rot.from_euler("xyz", rpy).as_matrix() M[:3, 3] = xyz return M def _mat(R, t): M = np.eye(4) M[:3, :3] = R.as_matrix() M[:3, 3] = t return M def servo_pose(xyz, rpy, axis, roll): """R_s, t_s placing our servo so +Y output -> joint axis (in parent frame), output pt at joint origin.""" d = Rot.from_euler("xyz", rpy).as_matrix() @ np.array(axis, float) d = d / np.linalg.norm(d) R_align, _ = Rot.align_vectors([d], [[0, 1, 0]]) # R_align @ +Y = d R_s = R_align * Rot.from_euler("y", roll) # roll about the output axis t_s = np.array(xyz, float) - R_s.apply(P0) return R_s, t_s def _mesh(link, fname, cname, R_s, t_s): v = ET.SubElement(link, "visual") r, p, y = R_s.as_euler("xyz") ET.SubElement(v, "origin", {"xyz": f"{t_s[0]:.6f} {t_s[1]:.6f} {t_s[2]:.6f}", "rpy": f"{r:.6f} {p:.6f} {y:.6f}"}) ET.SubElement(ET.SubElement(v, "geometry"), "mesh", {"filename": fname, "scale": SCALE}) ET.SubElement(ET.SubElement(v, "material", {"name": cname + str(id(v))}), "color", {"rgba": COLORS[cname]}) 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}), "color", {"rgba": rgba}) def _bar(link, A, B, thick, rgba, name): """Thick structural box spanning A->B (both in link frame, m).""" A, B = np.array(A, float), np.array(B, float) u = B - A L = np.linalg.norm(u) if L < 1e-6: return R, _ = Rot.align_vectors([u / L], [[1, 0, 0]]) # box local +X -> A->B direction c = (A + B) / 2 r, p, y = R.as_euler("xyz") v = ET.SubElement(link, "visual") ET.SubElement(v, "origin", {"xyz": f"{c[0]:.6f} {c[1]:.6f} {c[2]:.6f}", "rpy": f"{r:.6f} {p:.6f} {y:.6f}"}) ET.SubElement(ET.SubElement(v, "geometry"), "box", {"size": f"{L:.6f} {thick} {thick}"}) ET.SubElement(ET.SubElement(v, "material", {"name": name}), "color", {"rgba": rgba}) def gen(connect=False): robot = ET.Element("robot", {"name": "so100_para_connected" if connect else "so100_para"}) 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": "base"}) ET.SubElement(bj, "origin", {"xyz": "0 0 0", "rpy": "0 0 0"}) for i, name in enumerate(LINKS): link = ET.SubElement(robot, "link", {"name": name}) if i < len(JOINTS): # this link carries the servo for joint i _, _, _, xyz, rpy, axis, lo, hi = JOINTS[i] R_s, t_s = servo_pose(xyz, rpy, axis, ROLLS[i]) _mesh(link, SERVO, "servo", R_s, t_s) _mesh(link, HOLDER, "holder", R_s, t_s) if name == "base": _box(link, "0 -0.0452 -0.01", "0.08 0.08 0.006", "0.90 0.75 0.15 1", "baseplate") if name == "gripper": _box(link, "0.0135 -0.055 0", "0.012 0.05 0.02", "0.80 0.55 0.18 1", "fixed_jaw") if name == "jaw": _box(link, "0 -0.03 0", "0.012 0.05 0.02", "0.85 0.60 0.20 1", "moving_jaw") # PHASE 2: the YOKE connector (yoke_exact) gripping the PARENT servo output, at the SAME # relative pose as the single wrap_module (the yoke STL is authored in the servo frame, so its # pose = the parent servo's pose expressed in THIS child link's frame = inv(J_parent) @ M_servo). # It rotates with this link (the driven output), exactly like the locked module. # the jaw is an end-effector finger driven DIRECTLY by the gripper output — no yoke/bridge (that # clutter is only for arm joints that must carry the NEXT servo). if connect and 0 < i <= len(JOINTS) and name != "jaw": pj = JOINTS[i - 1] # parent joint (its servo we grip) Rp, tp = servo_pose(pj[3], pj[4], pj[5], ROLLS[i - 1]) P = np.linalg.inv(_T(pj[3], pj[4])) @ _mat(Rp, tp) yr, yp, yw = Rot.from_matrix(P[:3, :3]).as_euler("xyz") v = ET.SubElement(link, "visual") ET.SubElement(v, "origin", {"xyz": f"{P[0,3]:.6f} {P[1,3]:.6f} {P[2,3]:.6f}", "rpy": f"{yr:.6f} {yp:.6f} {yw:.6f}"}) ET.SubElement(ET.SubElement(v, "geometry"), "mesh", {"filename": YOKE, "scale": SCALE}) ET.SubElement(ET.SubElement(v, "material", {"name": "yk" + name}), "color", {"rgba": "0.85 0.60 0.20 1"}) # BRIDGE: real printable structural solid (so100_bridge.py) from the yoke far-end to THIS # link's holder, built in this child frame -> referenced at identity. Rolls were optimized # to keep it short. so the yoke actually connects to the next servo (load-bearing path). vb = ET.SubElement(link, "visual") ET.SubElement(vb, "origin", {"xyz": "0 0 0", "rpy": "0 0 0"}) ET.SubElement(ET.SubElement(vb, "geometry"), "mesh", {"filename": f"so100_bridge_{name}.stl", "scale": SCALE}) ET.SubElement(ET.SubElement(vb, "material", {"name": "br" + name}), "color", {"rgba": "0.80 0.55 0.18 1"}) for jname, parent, child, xyz, rpy, axis, lo, hi in JOINTS: j = ET.SubElement(robot, "joint", {"name": jname, "type": "revolute"}) ET.SubElement(j, "parent", {"link": parent}) ET.SubElement(j, "child", {"link": child}) ET.SubElement(j, "origin", {"xyz": f"{xyz[0]} {xyz[1]} {xyz[2]}", "rpy": f"{rpy[0]} {rpy[1]} {rpy[2]}"}) ET.SubElement(j, "axis", {"xyz": f"{axis[0]} {axis[1]} {axis[2]}"}) ET.SubElement(j, "limit", {"lower": str(lo), "upper": str(hi), "effort": "35", "velocity": "1"}) return robot if __name__ == "__main__": import sys connect = "connect" in sys.argv out = "so100_para_connected.urdf" if connect else "so100_para.urdf" tree = ET.ElementTree(gen(connect=connect)) ET.indent(tree, " ") tree.write(out, encoding="unicode", xml_declaration=True) print(f"wrote {out} (SO-100 chain, our servo+holder, 5DOF+gripper, connect={connect})")