"""Design exploration (pure Python): scale, attach point and carriage travel for the hanging arm. Coarse scene: deck underside plane, rail box, door inner face (local polyline), lining at x = -5 (pocket), end wall plane. Prints the best candidates. Run: python design_explore.py""" import json, math, os, sys, itertools HERE = os.path.dirname(os.path.abspath(__file__)) sys.path.insert(0, HERE) import design_kin as K ctx = json.load(open(os.path.join(HERE, "design_context.json"))) ctx2 = json.load(open(os.path.join(HERE, "design_context2.json"))) INNER = sorted({(f[0], f[1], f[2], f[3]) for f in ctx2["door_inner_faces_x0"]}) INNER = [f for f in INNER if f[3] > 0.3] # skip the lip faces at the top edge TRACK = {n: [tuple(r[1:4]) for r in ctx["door_track"][n]] for n in ctx["door_track"]} DECK_Z = -3.755 RAIL = dict(y0=-6.0238, y1=-5.5662, zb=-4.01, x_in=-2.5453, x_wall=-9.0348) # lining contour at x=-5 (pocket): points with z > -8.5 and y < -6 lin = [p for s in ctx["sections"]["Moonpool | hull lining"]["-5.0"] for p in s] LIN = sorted([tuple(p) for p in lin if p[1] > -9.0 and p[0] < -5.0], key=lambda p: -p[1]) def seg_dist2d(p, a, b): ax, ay = a; bx, by = b; px, py = p dx, dy = bx - ax, by - ay L2 = dx * dx + dy * dy t = 0 if L2 == 0 else max(0, min(1, ((px - ax) * dx + (py - ay) * dy) / L2)) return math.hypot(px - ax - t * dx, py - ay - t * dy) def door_face_dist(p_local_yz): """distance from a local (y,z) point to the door's inner face polyline; positive when above/inboard (in the free side)""" best = 1e9; sgn = 1 for i in range(len(INNER) - 1): a, b = INNER[i][:2], INNER[i + 1][:2] d = seg_dist2d(p_local_yz, a, b) if d < best: best = d n = (INNER[i][2], INNER[i][3]) sgn = 1 if (p_local_yz[0] - a[0]) * n[0] + (p_local_yz[1] - a[1]) * n[1] > 0 else -1 return best * sgn def lining_dist(yz): best = 1e9 for i in range(len(LIN) - 1): best = min(best, seg_dist2d(yz, LIN[i], LIN[i + 1])) # side: inside the hull = to the right (+y) of the contour at that z return best def face_point(u): """point and normal on the door's inner face at fraction u (0 top .. 1 bottom), local (y,z)""" f = u * (len(INNER) - 1) i = min(int(f), len(INNER) - 2) t = f - i a, b = INNER[i], INNER[i + 1] y = a[0] + t * (b[0] - a[0]); z = a[1] + t * (b[1] - a[1]) ny = a[2] + t * (b[2] - a[2]); nz = a[3] + t * (b[3] - a[3]) n = math.hypot(ny, nz) return (y, z), (ny / n, nz / n) def evaluate(s, u, px, d_extra, roll, cl_extra, door="Moonpool_Door_Aft", verbose=False, version="A", elbow=1): arm = K.Arm(s) Lc = 390 * s Hc = max(185 * s, 0.255 + 0.12) zb = -3.775 - Hc yb = (RAIL["y0"] + RAIL["y1"]) / 2 side = +1 if door.endswith("Aft") else -1 # travel tr = TRACK[door] xs = [p[0] for p in tr] x_cl, x_op = tr[0][0], (min(xs) if side > 0 else max(xs)) if side > 0: c_hi = RAIL["x_in"] - 0.04 - Lc / 2 c_lo = RAIL["x_wall"] + 0.04 + Lc / 2 + cl_extra else: c_hi = 3.05 * 2 - (RAIL["x_in"] - 0.04 - Lc / 2) c_lo = 3.05 * 2 - (RAIL["x_wall"] + 0.04 + Lc / 2 + cl_extra) (fy, fz), (ny, nz) = face_point(u) d_t = ((60 + 75 + 115) * s + 0.08 if version == "A" else (60 + 70 + 20) * s + 0.02) + d_extra # tool axes (world): approach x_t = -n (into the door), roll: y_t in the plane perpendicular n_w = (0.0, ny, nz) xt = (0.0, -ny, -nz) a = (1.0, 0.0, 0.0) # door's x direction lies in the face plane b = K.cross(xt, a) yt = tuple(math.cos(roll) * a[i] + math.sin(roll) * b[i] for i in range(3)) zt = K.cross(xt, yt) tool_c = (K.w2c(xt), K.w2c(yt), K.w2c(zt)) pxl = px * side res = dict(fail=None, q=[], minq5=9, elb=[9, -9], rod=[9, -9], cl=9, clwhere="", q1=[9, -9], q2=[9, -9], q3=[9, -9], q4=[9, -9], q6=[9, -9]) prev = None for f, (dx, dy, dz) in enumerate(tr, start=1): # carriage proportional to the door's slide c = c_lo + (dx - x_op) * (c_hi - c_lo) / (x_cl - x_op) if side > 0 else c_lo + (dx - x_op) * (c_hi - c_lo) / (x_cl - x_op) P = (dx + pxl, dy + fy, dz + fz) WC = (P[0] - d_t * xt[0], P[1] - d_t * xt[1], P[2] - d_t * xt[2]) wc_c = K.w2c((WC[0] - c, WC[1] - yb, WC[2] - zb)) q = K.solve(arm, wc_c, tool_c, branch5=+1) if q is None: res["fail"] = "unreachable f%d" % f return res if prev is not None: for k in ("q1", "q4", "q6"): if abs(q[k] - prev[k]) > 0.5: res["fail"] = "jump %s f%d" % (k, f) return res prev = q res["minq5"] = min(res["minq5"], abs(q["q5"])) res["elb"] = [min(res["elb"][0], q["elbow_inner"]), max(res["elb"][1], q["elbow_inner"])] for k in ("q1", "q2", "q3", "q4", "q6"): res[k] = [min(res[k][0], q[k]), max(res[k][1], q[k])] t, kk, L = arm.rod(q["phi_l"], q["phi_f"]) res["rod"] = [min(res["rod"][0], L), max(res["rod"][1], L)] # clearance of key points (world): J3 elbow, WC, mid lower arm, mid forearm F = K.fk(arm, q) base = (c, yb, zb) pts = {} for nm in ("J2", "J3", "WC"): pts[nm] = tuple(base[i] + K.c2w(F[nm])[i] for i in range(3)) pts["mid_l"] = tuple((pts["J2"][i] + pts["J3"][i]) / 2 for i in range(3)) pts["mid_f"] = tuple((pts["J3"][i] + pts["WC"][i]) / 2 for i in range(3)) rad = {"J2": 130 * s, "J3": 110 * s, "WC": 90 * s, "mid_l": 120 * s, "mid_f": 100 * s} for nm, p in pts.items(): cl_deck = DECK_Z - p[2] - rad[nm] loc = (p[1] - dy, p[2] - dz) cl_door = door_face_dist(loc) - rad[nm] if (side > 0 and p[0] < -3.1) or (side < 0 and p[0] > 9.2): cl_lin = lining_dist((p[1], p[2])) - rad[nm] else: cl_lin = 9 cl_wall = (p[0] - RAIL["x_wall"] - rad[nm]) if side > 0 else (15.1348 - p[0] - rad[nm]) # rail: points near the rail's y and z for v, w in ((cl_deck, "deck"), (cl_door, "door"), (cl_lin, "lining"), (cl_wall, "wall")): if v < res["cl"]: res["cl"] = v res["clwhere"] = "%s %s f%d" % (nm, w, f) res["q"].append(q) res.update(dict(c_lo=c_lo, c_hi=c_hi, T=c_hi - c_lo, Lc=Lc, Hc=Hc, zb=zb, d_t=d_t, face=(fy, fz), n=(ny, nz))) return res def score(r): if r["fail"]: return -1e9 sc = 0 sc += min(r["minq5"], math.radians(45)) * 4 # wrist away from its singularity sc += min(r["elb"][0], math.radians(60)) * 2 - max(0, r["elb"][1] - math.radians(150)) * 4 sc += min(r["cl"], 0.25) * 10 sc -= (r["q3"][1] - r["q3"][0]) * 0.5 return sc if __name__ == "__main__" and len(sys.argv) == 1: best = [] for s in (0.0016, 0.0018, 0.0020, 0.0022): for u in [i / 20 for i in range(0, 15)]: for px in (-2.7, -2.5, -2.3, -2.1, -1.9): for roll in (0.0, math.pi / 2): for cle in (0.25, 0.45): r = evaluate(s, u, px, 0.0, roll, cle) sc = score(r) if sc > -1e8: best.append((sc, s, u, px, roll, cle, r)) best.sort(key=lambda t: -t[0]) for sc, s, u, px, roll, cle, r in best[:25]: print("sc %.2f s %.4f u %.2f px %.2f roll %.2f cle %.2f | q5min %.0f elb %.0f..%.0f cl %.3f (%s) T %.2f rod %.2f..%.2f q1 %.0f..%.0f q2 %.0f..%.0f q3 %.0f..%.0f face %s" % ( sc, s, u, px, roll, cle, math.degrees(r["minq5"]), math.degrees(r["elb"][0]), math.degrees(r["elb"][1]), r["cl"], r["clwhere"], r["T"], r["rod"][0], r["rod"][1], math.degrees(r["q1"][0]), math.degrees(r["q1"][1]), math.degrees(r["q2"][0]), math.degrees(r["q2"][1]), math.degrees(r["q3"][0]), math.degrees(r["q3"][1]), tuple(round(v, 2) for v in r["face"]))) print(len(best), "feasible") def stroke_ok(r, s): lo, hi = r["rod"] S = hi - lo return lo - S # >= dead length needed (end-trunnion cylinder) def explore2(): out = [] for s in (0.0015, 0.0016, 0.0017, 0.0018, 0.0019, 0.0020, 0.0021, 0.0022, 0.0024): for u in [i / 40 for i in range(0, 30)]: for px in (-2.75, -2.6, -2.45, -2.3, -2.15, -2.0, -1.85, -1.7): for cle in (0.2, 0.35, 0.5): ra = evaluate(s, u, px, 0.0, 0.0, cle, version="A") rb = evaluate(s, u, px, 0.0, 0.0, cle, version="B") if ra["fail"] or rb["fail"]: continue for door in ("Moonpool_Door_Fore",): rf = evaluate(s, u, px, 0.0, 0.0, cle, door=door, version="A") if rf["fail"]: ra["fail"] = rf["fail"] if ra["fail"]: continue dead = min(stroke_ok(ra, s), stroke_ok(rb, s)) q3r = max(ra["q3"][1], rb["q3"][1]) - min(ra["q3"][0], rb["q3"][0]) q2r = max(ra["q2"][1], rb["q2"][1]) - min(ra["q2"][0], rb["q2"][0]) sc = (min(ra["minq5"], rb["minq5"], math.radians(40)) * 3 + min(ra["cl"], rb["cl"], 0.25) * 12 + min(dead, 0.25 * s / 0.002) * 8 - q3r * 0.6 - q2r * 0.3 + min(ra["elb"][0], rb["elb"][0], math.radians(50)) * 1.5) out.append((sc, s, u, px, cle, ra, rb, dead, q2r, q3r)) out.sort(key=lambda t: -t[0]) for sc, s, u, px, cle, ra, rb, dead, q2r, q3r in out[:30]: print("sc %.2f s %.4f u %.3f px %.2f cle %.2f | dead %.3f q2r %.0f q3r %.0f q5min %.0f/%.0f cl %.3f/%.3f (%s | %s) elb %.0f..%.0f T %.2f rodA %.2f..%.2f face %s n %s" % ( sc, s, u, px, cle, dead, math.degrees(q2r), math.degrees(q3r), math.degrees(ra["minq5"]), math.degrees(rb["minq5"]), ra["cl"], rb["cl"], ra["clwhere"], rb["clwhere"], math.degrees(min(ra["elb"][0], rb["elb"][0])), math.degrees(max(ra["elb"][1], rb["elb"][1])), ra["T"], ra["rod"][0], ra["rod"][1], tuple(round(v, 2) for v in ra["face"]), tuple(round(v, 2) for v in ra["n"]))) print(len(out), "feasible") if __name__ == "__main__" and len(sys.argv) > 1 and sys.argv[1] == "2": explore2()