"""Door robot arm: the fit of the scaled-up arm (2026-10-18; plain Python, no Blender). The user: "make it bigger so those can withstand the pressure to open that door under the water" and "attach the main plate to the doors center and make it bigger". The whole machine is scaled uniformly by S about the rail's line under the deck (the anchor: the rail's centre y and the deck's underside z, so the rail stays set into the deck, the chain tray's hangers stay on the deck and the carriage hangs under the rail as before); the fixtures go to the doors' centres (build/door_centre.json). For a scale S and the carriage's travel this script solves the arm at every quarter frame of both doors (build/door_path.json) for both versions with the rig's own inverse kinematics and reports: joint angles and their margins to the limits, how far the arm is from fully stretched, the J3 cylinder's length, the arm's clearance to the pocket's end wall and to the door's inner face (rough: link radii), the carriage's travel. python build/dra_fit.py [S ...] -> a table; python build/dra_fit.py --write S c_open_AFT writes design.json (from design_base.json + the scale, the attach points, the schedule) and build/fit_report.json. """ import os import sys import json import math import copy HERE = os.path.dirname(os.path.abspath(__file__)) ROOT = os.path.dirname(HERE) BASE = json.load(open(os.path.join(ROOT, "design_base.json"))) DC = json.load(open(os.path.join(HERE, "door_centre.json"))) DP = json.load(open(os.path.join(HERE, "door_path.json"))) DECK = BASE["scene"]["deck_underside_z"] # -3.755: the anchor's z YR = BASE["carriage"]["centre_y"] # -5.795: the rail's centre (the anchor's y) HALF0 = BASE["carriage"]["size"]["along_rail"] / 2 # 0.429 GAP_INNER = 0.040 # the carriage's body to the rail's inner end at the closed door PLATE_B = 1.20 # version B's plate (m, square) PAD_B = 1.30 # its conformal pad on the door PAD_MIN = 0.012 # the pad's least thickness (at its edges on the hollow door) LIMITS = {"J1": (-170, 170), "J2": (-95, 45), "J3": (-40, 55), "J4": (-170, 170), "J5": (-120, 120), "J6": (-170, 170)} def sag(size): """how far the door's inner face rises toward the arm at the edge of a flat square of this size (door_centre.json)""" t = sorted((float(k), v[1]) for k, v in DC["AFT"]["sag_under_plate_m"].items()) for (a, va), (b, vb) in zip(t, t[1:]): if a <= size <= b: return va + (vb - va) * (size - a) / (b - a) return t[-1][1] * (size / t[-1][0]) ** 2 def tool_axes(n): xt = [-n[0], -n[1], -n[2]] yt = [-1.0, 0.0, 0.0] zt = [xt[1] * yt[2] - xt[2] * yt[1], xt[2] * yt[0] - xt[0] * yt[2], xt[0] * yt[1] - xt[1] * yt[0]] C = lambda v: [v[0], -v[1] + 0.0, -v[2] + 0.0] return dict(x_t=xt, y_t=yt, z_t=zt), dict(x_t=C(xt), y_t=C(yt), z_t=C(zt)) def pad_top_B(): return round(sag(PAD_B) + PAD_MIN, 4) def d_t(S): """wrist centre to the door face along its normal: A all parts scaled; B: flange + neck + plate scaled, the pad sized to the door (its top above the hollow face's highest point under it)""" return {"A": round(0.63 * S, 5), "B": round((0.326 - 0.020) * S + pad_top_B(), 5)} def scaled(S, c_open=None): D = copy.deepcopy(BASE) sc = lambda v: [x * S for x in v] K = D["kinematics"] for j in K["joints"].values(): j["origin"] = sc(j["origin"]) for k in ("L2", "d4", "a3", "L3", "j2_r", "j2_h", "flange_from_wc"): K[k] = K[k] * S K["wc_rest_A"], K["j3_rest_A"] = sc(K["wc_rest_A"]), sc(K["j3_rest_A"]) r = K["rod"] for k in ("trunnion_rest_A", "knob_rest_A", "trunnion_in_lower", "knob_in_upper"): r[k] = sc(r[k]) r["length_rest"] = r["length_rest"] * S car = D["carriage"] car["size"] = {k: (v * S if isinstance(v, (int, float)) else v) for k, v in car["size"].items()} car["underside_z"] = round(DECK + S * (BASE["carriage"]["underside_z"] - DECK), 5) car["top_z"] = round(DECK + S * (BASE["carriage"]["top_z"] - DECK), 5) dt = d_t(S) for key in ("AFT", "FORE"): dr = D["doors"][key] c = DC[key] dr["attach_local"] = c["attach_local"] dr["normal_local"] = c["normal_local"] dr["tool_axes_world"], dr["tool_axes_C"] = tool_axes(c["normal_local"]) dr["wc_local"] = {v: [c["attach_local"][i] + dt[v] * c["normal_local"][i] for i in range(3)] for v in ("A", "B")} sg = 1 if key == "AFT" else -1 rail = D["scene"]["rail"]["usable_" + key] c_closed = (rail[1] - GAP_INNER - HALF0 * S) if key == "AFT" else (rail[0] + GAP_INNER + HALF0 * S) co = c_open if key == "AFT" else (2 * D["scene"]["mirror_plane_x"] - c_open if c_open is not None else None) if co is None: co = BASE["doors"][key]["carriage"]["c_open"] B = (c_closed - co) / (dr["closed_x"] - dr["open_x"]) A = c_closed - B * dr["closed_x"] dr["carriage"] = dict(c_closed=round(c_closed, 4), c_open=round(co, 4), travel=round(abs(c_closed - co), 4), formula=BASE["doors"][key]["carriage"]["formula"], A=round(A, 6), B=round(B, 6), note=BASE["doors"][key]["carriage"]["note"]) car["travel_ends"][key] = sorted([round(c_closed, 4), round(co, 4)]) return D, dt def solve(D, key, ver, dt, door_loc): """dra_ik.solve with this version's wrist centre; also the points of the arm (world) for the clearances""" K = D["kinematics"] dr = D["doors"][key] wc = dr["wc_local"][ver] yb, zb = D["carriage"]["centre_y"], D["carriage"]["underside_z"] A, B = dr["carriage"]["A"], dr["carriage"]["B"] dx, dy, dz = door_loc c = A + B * dx X = (1 - B) * dx + (wc[0] - A) Y = (yb - wc[1]) - dy H = (zb - wc[2]) - dz q1 = math.atan2(Y, X) R = math.hypot(X, Y) + K["j2_r"] Hh = H - K["j2_h"] L2, L3, beta, phi_l0 = K["L2"], K["L3"], K["beta_rad"], K["phi_l0_rad"] Dd = math.hypot(R, Hh) cc_raw = (L2 * L2 - L3 * L3 + Dd * Dd) / (2 * L2 * Dd) cc = max(-1.0, min(1.0, cc_raw)) phi_l = math.atan2(Hh, R) + math.acos(cc) q2 = phi_l - phi_l0 phi_f = math.atan2(Hh - L2 * math.sin(phi_l), R - L2 * math.cos(phi_l)) - beta q3 = phi_f - phi_l + phi_l0 xt, yt, zt = dr["tool_axes_C"]["x_t"], dr["tool_axes_C"]["y_t"], dr["tool_axes_C"]["z_t"] P = lambda v: math.cos(q1) * v[0] + math.sin(q1) * v[1] M11 = math.cos(phi_f) * P(xt) + math.sin(phi_f) * xt[2] M21 = -math.sin(q1) * xt[0] + math.cos(q1) * xt[1] M31 = math.cos(phi_f) * xt[2] - math.sin(phi_f) * P(xt) M12 = math.cos(phi_f) * P(yt) + math.sin(phi_f) * yt[2] M13 = math.cos(phi_f) * P(zt) + math.sin(phi_f) * zt[2] q5 = -math.acos(max(-1.0, min(1.0, M11))) q4 = math.atan2(-M21, M31) q6 = math.atan2(-M12, -M13) rod = K["rod"] tl = rod["trunnion_in_lower"] j3l = [K["joints"]["J3"]["origin"][i] - K["joints"]["J2"]["origin"][i] for i in range(3)] ka, kb = rod["knob_in_upper"][0], rod["knob_in_upper"][2] kx = (j3l[0] - tl[0]) + ka * math.cos(q3) - kb * math.sin(q3) kz = (j3l[2] - tl[2]) + ka * math.sin(q3) + kb * math.cos(q3) L = math.hypot(kx, kz) # world points: plane coordinates (r from the J1 axis, h up in the arm frame) -> world def W(r, h): return (c + r * math.cos(q1), yb - r * math.sin(q1), zb - h) j2 = (-K["j2_r"], K["j2_h"]) j3 = (j2[0] + L2 * math.cos(phi_l), j2[1] + L2 * math.sin(phi_l)) w = (j2[0] + R, j2[1] + Hh) pts = dict(J2=W(*j2), J3=W(*j3), WC=W(*w)) seg = [] for (a, b, rad) in ((j2, j3, 0.30 * S_of(D)), (j3, w, 0.22 * S_of(D))): for k in range(9): t = k / 8 seg.append((W(a[0] + (b[0] - a[0]) * t, a[1] + (b[1] - a[1]) * t), rad)) q = dict(J1=q1, J2=q2, J3=q3, J4=q4, J5=q5, J6=q6) return dict(q={j: math.degrees(v) for j, v in q.items()}, c=c, L=L, stretch=cc_raw, pts=pts, seg=seg) def S_of(D): return D["kinematics"]["L2"] / BASE["kinematics"]["L2"] def door_curve(key, loc): """the door's inner curve in world (y, z), and the normals toward the arm""" c = DC[key]["inner_curve_yz"] return [(loc[1] + y, loc[2] + z) for y, z in c] def side_dist(p, curve): """distance (y-z) from p to the door's inner curve, negative when p is behind the face (inside the door)""" best = (9.0, 1.0) for (a, b) in zip(curve, curve[1:]): ay, az = a by, bz = b ey, ez = by - ay, bz - az L2 = ey * ey + ez * ez t = max(0.0, min(1.0, ((p[1] - ay) * ey + (p[2] - az) * ez) / L2)) if L2 > 0 else 0.0 qy, qz = ay + ey * t, az + ez * t d = math.hypot(p[1] - qy, p[2] - qz) if d < best[0]: ny, nz = -ez, ey # left normal of the curve's direction s = (p[1] - qy) * ny + (p[2] - qz) * nz best = (d, 1.0 if s >= 0 else -1.0) return best[0] * best[1] def evaluate(S, c_open, verbose=False): D, dt = scaled(S, c_open) # which side of the curve is the arm's: the inner normal at the centre out = dict(S=S, c_open=c_open, d_t=dt, root_z=D["carriage"]["underside_z"], carriage={k: D["doors"][k]["carriage"] for k in ("AFT", "FORE")}) rng, worst = {}, {} stretch, Lr = 0.0, [9, 0] wall, door = 9.0, 9.0 for key in ("AFT", "FORE"): n = DC[key]["normal_local"] sgn_side = None ew = D["scene"]["end_wall_faces_x"][key] for ver in ("A", "B"): for f, x, y, z in DP[key]: s = solve(D, key, ver, dt, (x, y, z)) for j, v in s["q"].items(): lo_hi = rng.setdefault(ver + j, [v, v]) lo_hi[0], lo_hi[1] = min(lo_hi[0], v), max(lo_hi[1], v) stretch = max(stretch, s["stretch"]) Lr = [min(Lr[0], s["L"] / S), max(Lr[1], s["L"] / S)] # the end wall: the carriage's back, the arm's points back = s["c"] - HALF0 * S if key == "AFT" else s["c"] + HALF0 * S xs = [back] + [p[0] - (0.30 * S if key == "AFT" else -0.30 * S) for p in s["pts"].values()] gap = min(x_ - ew for x_ in xs) if key == "AFT" else min(ew - x_ for x_ in xs) wall = min(wall, gap) # the door's inner face (only where the door is along x) curve = door_curve(key, (x, y, z)) if sgn_side is None: att = (y + DC[key]["attach_local"][1] + n[1] * 0.3, z + DC[key]["attach_local"][2] + n[2] * 0.3) sgn_side = 1.0 if side_dist((0, att[0], att[1]), curve) > 0 else -1.0 lx0, lx1 = DC[key]["length_x"] for (p, rad) in s["seg"][:-2]: # the tool's end sits on its fixture by design if x + lx0 - rad <= p[0] <= x + lx1 + rad: door = min(door, sgn_side * side_dist(p, curve) - rad) for j in ("J1", "J2", "J3", "J4", "J5", "J6"): lo, hi = LIMITS[j] for ver in ("A", "B"): a, b = rng[ver + j] worst[ver + j] = round(min(a - lo, hi - b), 1) out.update(ranges={k: [round(v[0], 1), round(v[1], 1)] for k, v in rng.items()}, margins=worst, stretch_max=round(stretch, 4), rod_L_over_S=[round(Lr[0], 4), round(Lr[1], 4)], end_wall_gap=round(wall, 3), door_gap=round(door, 3)) return out, D if __name__ == "__main__": args = sys.argv[1:] if args and args[0] == "--write": S, co = float(args[1]), float(args[2]) rep, D = evaluate(S, co) D["scale_up"] = dict(factor=S, anchor_yz=[YR, DECK], date="2026-10-18", why="the user: make it bigger to withstand the pressure; the door's centre is 4.1 m under " "the rail; see build/fit_report.json", plate_B=PLATE_B, pad_B=PAD_B, pad_B_top=pad_top_B(), d_t=d_t(S)) D["tools"]["A"]["d_t"], D["tools"]["B"]["d_t"] = d_t(S)["A"], d_t(S)["B"] D["tools"]["B"]["plate"] = [PLATE_B, PLATE_B, round(0.044 * S, 4)] with open(os.path.join(ROOT, "design.json"), "w") as fh: json.dump(D, fh, indent=1) with open(os.path.join(HERE, "fit_report.json"), "w") as fh: json.dump(rep, fh, indent=1) print(json.dumps(rep, indent=1)) else: Ss = [float(a) for a in args] or [1.3, 1.4, 1.5, 1.6] for S in Ss: for co in (-8.4, -8.2, -8.0, -7.8, -7.6, -7.4): rep, _ = evaluate(S, co) print("S %.2f c_open %.2f root %.3f c_closed %.3f | margins %s | stretch %.3f rodL/S %s wall %.2f door %.2f" % ( S, co, rep["root_z"], rep["carriage"]["AFT"]["c_closed"], " ".join("%s%s" % (k, v) for k, v in rep["margins"].items() if k[1:] in ("J2", "J3", "J5")), rep["stretch_max"], rep["rod_L_over_S"], rep["end_wall_gap"], rep["door_gap"]))