"""Parametric thumb-style gripper (path B) — STANDALONE test rig, NOT wired into the main arm yet. One servo: the horn drives a thumb finger that closes against a fixed jaw mounted on the holder. Servo frame (mm): output axis +Y through (X=AX=-25.5, Z=0); horn on +Y face (Y=9.6), idler -Y (Y=-28.2). Fingers extend in -X and close in Z (rotation about +Y). Flush-at-closed is automatic: both fingers are authored at the CLOSED pose (thumb pad at +GRIP_GAP/2 facing -Z, fixed pad at -GRIP_GAP/2 facing +Z, parallel) and the jaw joint OPENS by rotating the thumb up about the output axis. Reuses st3215.stl (servo) + holder_approx_drilled.stl (holder). Outputs thumb.stl, fixed_jaw.stl, gripper_testing.urdf. Iterate via gripper_render.py (open vs closed). """ import numpy as np import trimesh import xml.etree.ElementTree as ET # ---- interface (fixed by the servo) ---- AX, AZ = -25.5, 0.0 # output axis (X, Z); axis dir +Y HORN_Y, IDLER_Y = 9.6, -28.2 # ---- gripper params (tune these while iterating) ---- GRIP_GAP = 4.0 # gap between pads at full close (mm) FINGER_LEN = 60.0 # finger reach in -X (mm) FY0, FY1 = 11.0, 31.0 # finger Y span (mm) — IN FRONT of the servo face (Y=9.6) so the fingers # clear the body; both fingers share this so they grip the same volume FT = 6.0 # finger thickness in Z (mm) PAD_STYLE = "vgroove" # fixed-jaw contact: "vgroove" (cradles round objects) or "flat" GV = 5.0 # V-groove diamond size (mm) ROOT_X = AX + 6.0 # finger root X (slightly past the pivot toward the body) TIP_X = AX - FINGER_LEN # finger tip X OPEN_ANGLE = 0.9 # rad, jaw fully open (0 = closed) # ---- thumb finger style ---- FINGER_STYLE = "finray" # "finray" (compliant TPU, Cameron's direction) or "box" (rigid v1 fallback) FR_H = 16.0 # fin-ray base height (Z) at the root; tapers to the tip FR_WALL = 2.0 # wall / rib thickness (mm) — thin for TPU compliance FR_RIBS = 5 # number of internal ribs # ---- real STS3215 horn interface (exact pattern from yoke_exact.py) ---- PT = 4.0 # horn plate thickness (Y) HOLES_XZ = [(-32.5, 0.0), (-25.5, 7.0), (-25.5, -7.0), (-18.5, 0.0)] # 4 horn bolt holes (X,Z) BOLT_D, BORE_D = 2.9, 9.0 # bolt clearance dia, center bore dia (horn boss) def _box(x0, x1, y0, y1, z0, z1): b = trimesh.creation.box(extents=[x1 - x0, y1 - y0, z1 - z0]) b.apply_translation([(x0 + x1) / 2, (y0 + y1) / 2, (z0 + z1) / 2]) return b def _cyl_Y(x, z, dia, y0, y1): """A cylinder of diameter `dia` along the Y axis, centred at (x, z), spanning Y[y0,y1].""" c = trimesh.creation.cylinder(radius=dia / 2.0, height=y1 - y0, sections=32) c.apply_transform(trimesh.transformations.rotation_matrix(np.pi / 2, [1, 0, 0])) # Z-axis -> Y c.apply_translation([x, (y0 + y1) / 2.0, z]) return c def _horn_plate(): """Real horn mount: a plate on the horn face (Y=9.6..) drilled with the exact 4-bolt pattern + center bore, so the thumb bolts precisely to the STS3215 output horn (the 'precise mating' ask).""" y0, y1 = HORN_Y, FY0 + 4.0 # reach forward to overlap the finger base (Y=11..15) plate = _box(-36, -15, y0, y1, -11, 11) tools = [_cyl_Y(hx, hz, BOLT_D, y0 - 1, y1 + 1) for hx, hz in HOLES_XZ] tools.append(_cyl_Y(AX, AZ, BORE_D, y0 - 1, y1 + 1)) return trimesh.boolean.difference([plate] + tools, engine="manifold") def _finray_profile(L, H, wall, n_ribs): """2D fin-ray outline in (length u, height v): front (contact) edge along v=0, base along u=0, spine = hypotenuse (0,H)->(L,0). Hollow outer walls + angled internal ribs. Returns a shapely polygon.""" from shapely.geometry import Polygon, LineString from shapely.ops import unary_union tri = Polygon([(0, 0), (L, 0), (0, H)]) parts = [tri.difference(tri.buffer(-wall))] # thin outer ring (spine + front + base walls) for k in range(1, n_ribs + 1): x_f = L * k / (n_ribs + 1) # rib foot on the front edge (v=0) x_s = x_f * 0.55 # rib top leans back toward the base y_s = H * (1 - x_s / L) # land on the spine (hypotenuse) parts.append(LineString([(x_f, 0.0), (x_s, y_s)]).buffer(wall / 2.0)) return unary_union(parts) def build_thumb(): """Moving finger driven by the horn. FINGER_STYLE='finray' -> compliant TPU triangle (front/contact edge at Z=+GRIP_GAP/2 facing -Z, spine up); 'box' -> rigid v1. Plus a clamp bridging the base to the horn (+Y).""" zf = GRIP_GAP / 2.0 plate = _horn_plate() # real horn bolt-on mount gusset = _box(-36, -15, FY0, FY0 + 4, zf, 11) # tie the plate up into the finger base if FINGER_STYLE == "finray": prof = _finray_profile(FINGER_LEN, FR_H, FR_WALL, FR_RIBS) fr = trimesh.creation.extrude_polygon(prof, FY1 - FY0) # local: X=len, Y=height, Z=width M = np.array([[-1, 0, 0, ROOT_X], [0, 0, 1, FY0], [0, 1, 0, zf], [0, 0, 0, 1]], float) fr.apply_transform(M) # -> servo frame (len->-X, height->Z, width->Y) return trimesh.boolean.union([fr, plate, gusset], engine="manifold") finger = _box(TIP_X, ROOT_X, FY0, FY1, zf, zf + FT) return trimesh.boolean.union([finger, plate, gusset], engine="manifold") def build_fixed(): """Lower finger (contact face at -GRIP_GAP/2, facing +Z) + an L-bracket routed UNDER the servo body (Z < body min -12.4) back to the holder so it doesn't intersect the servo.""" zf = -GRIP_GAP / 2.0 finger = _box(TIP_X, -40, FY0, FY1, zf - FT, zf) # lower finger, X<-40 to clear the thumb horn plate strut = _box(-44, -38, FY0, FY1, -15, zf) # drop in FRONT of the body (Y>9.6), clear of plate back = _box(-44, 12, -6, FY1, -15, -13) # cross UNDER the body (Z<-12.4) to the holder jaw = trimesh.boolean.union([finger, strut, back], engine="manifold") if PAD_STYLE == "vgroove": # cut a V-groove along the finger to cradle round objects gv = trimesh.creation.box(extents=[FINGER_LEN + 10, GV, GV]) gv.apply_transform(trimesh.transformations.rotation_matrix(np.pi / 4, [1, 0, 0])) # diamond in Y-Z gv.apply_translation([(TIP_X - 40) / 2.0, (FY0 + FY1) / 2.0, zf]) # notch into the contact face jaw = trimesh.boolean.difference([jaw, gv], engine="manifold") return jaw def _vis(link, fn, col, off="0 0 0"): 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}) def gen_urdf(path="gripper_testing.urdf"): robot = ET.Element("robot", {"name": "gripper_testing"}) ET.SubElement(robot, "link", {"name": "world"}) wj = ET.SubElement(robot, "joint", {"name": "world_to_base", "type": "fixed"}) ET.SubElement(wj, "parent", {"link": "world"}) ET.SubElement(wj, "child", {"link": "base"}) ET.SubElement(wj, "origin", {"xyz": "0 0 0", "rpy": "1.5708 0 0"}) # Y-up -> Z-up (match arm viewer) base = ET.SubElement(robot, "link", {"name": "base"}) _vis(base, "st3215.stl", "0.35 0.35 0.40 1") _vis(base, "gripper_base.stl", "0.20 0.52 0.85 1") # holder + fixed jaw, unioned jj = ET.SubElement(robot, "joint", {"name": "jaw", "type": "revolute"}) ET.SubElement(jj, "parent", {"link": "base"}) ET.SubElement(jj, "child", {"link": "thumb"}) ET.SubElement(jj, "origin", {"xyz": f"{AX/1000:.5f} 0 0", "rpy": "0 0 0"}) ET.SubElement(jj, "axis", {"xyz": "0 1 0"}) ET.SubElement(jj, "limit", {"lower": "-0.1", "upper": f"{OPEN_ANGLE}", "effort": "3", "velocity": "3"}) thumb = ET.SubElement(robot, "link", {"name": "thumb"}) _vis(thumb, "thumb.stl", "0.90 0.65 0.20 1", f"{-AX/1000:.5f} 0 0") # servo-frame STL in output link ET.ElementTree(robot).write(path) print("wrote", path) if __name__ == "__main__": t, f = build_thumb(), build_fixed() t.export("thumb.stl") f.export("fixed_jaw.stl") holder = trimesh.load("holder_approx_drilled.stl", force="mesh") base = trimesh.boolean.union([holder, f], engine="manifold") # one printable base part base.export("gripper_base.stl") nb = len(base.split(only_watertight=False)) print(f"thumb: bbox {np.round(t.extents,1).tolist()} wt={t.is_watertight} " f"fixed: bbox {np.round(f.extents,1).tolist()}") print(f"gripper_base: vol={base.volume/1000:.1f}cm3 wt={base.is_watertight} bodies={nb}" f"{' (WARNING disjoint)' if nb > 1 else ''}") gen_urdf()