API tools faq
paste
Hadlock

ai assisted dynamic robot leg generator

Hadlock
Apr 22nd, 2026
140
0
Never
9
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 89.61 KB | Source Code | 0 0
raw download clone embed print report
  1. import pygame
  2. import math
  3. import sys
  4. import json
  5. import time
  6. import os
  7.  
  8. pygame.init()
  9.  
  10. WIDTH, HEIGHT = 1100, 800
  11. screen = pygame.display.set_mode((WIDTH, HEIGHT))
  12. pygame.display.set_caption("AT-AT Quadruped — Klann Linkage Gait")
  13. clock = pygame.time.Clock()
  14.  
  15. # --- Colors ---
  16. BG           = (15, 18, 25)
  17. BODY_COLOR   = (80, 90, 105)
  18. BODY_DARK    = (55, 62, 75)
  19. LEG_COLOR    = (100, 110, 125)
  20. LEG_FAR      = (68, 75, 88)
  21. JOINT_COLOR  = (200, 210, 220)
  22. JOINT_FAR    = (130, 140, 150)
  23. FOOT_COLOR   = (70, 80, 95)
  24. LINK_COLOR   = (140, 155, 170)
  25. LINK_FAR     = (95, 105, 118)
  26. CRANK_COLOR  = (180, 140, 80)
  27. SLIDER_BG    = (35, 40, 50)
  28. SLIDER_FG    = (100, 140, 180)
  29. SLIDER_HAND  = (160, 200, 230)
  30. TEXT_COLOR    = (180, 190, 200)
  31. TEXT_DIM      = (100, 110, 120)
  32. GRID_COLOR   = (25, 30, 40)
  33. GROUND_COLOR = (40, 50, 60)
  34. TRACE_NEAR   = (180, 80, 80)
  35. TRACE_FAR    = (80, 80, 180)
  36. SAFETY_ORANGE = (255, 102, 0)
  37. AMBER        = (220, 160, 40)
  38.  
  39. PX_PER_INCH = 20
  40. PX_PER_M    = PX_PER_INCH / 0.0254   # ~787.4 px per meter
  41.  
  42. # --- Physics constants ---
  43. LEG_MASS   = 0.350    # kg per leg (PLA + 2 servos + bearings + hardware)
  44. BODY_MASS  = 0.600    # kg chassis, battery, wiring, compute
  45. TOTAL_MASS = BODY_MASS + 4 * LEG_MASS  # 2.0 kg
  46. GRAVITY    = 9.81     # m/s^2
  47. MU_JOINT   = 0.015    # bearing/joint friction coefficient
  48. MU_GROUND  = 0.25     # foot-ground sliding friction (during swing retract)
  49. JOINT_RAD_M = 0.003   # effective joint pin radius ~3mm
  50.  
  51. # ============================================================
  52. # Klann Patent 6-bar Linkage (Stephenson III)
  53. #
  54. # Reference: diywalkers.com/klanns-linkage-plans.html
  55. #
  56. # Fixed frame pivots (relative to crank center J0):
  57. #   J3 (lower): (-6.6, -1.97)  in math coords (Y up)
  58. #   J5 (upper): (-2.6, +6.9)
  59. #
  60. # Moving joints:
  61. #   J1 = crank pin (rotates around J0, radius B0)
  62. #   J4 = circle intersection: (J1,B2) & (J3,B3)  [high]
  63. #   J6 = rigid bent extension from J4 away from J1
  64. #        length B4, bend angle -B6_angle
  65. #   J7 = circle intersection: (J5,B6) & (J6,B5)  [high]
  66. #   J8 = foot, rigid bent extension from J6 away from J7
  67. #        length B7, bend angle +B7_angle
  68. #
  69. # Patent bar lengths (crank=3 base):
  70. #   B0=3  B2=6.6  B3=3.59  B4=5.84
  71. #   B5=10.04  B6=5.79  B7=10.04
  72. #
  73. # Bend angles: B6_angle=12.8°  B7_angle=30°
  74. # ============================================================
  75.  
  76. SIM_SCALE = 7
  77.  
  78. DEFAULT_PARAMS = {
  79.     "crank":     3.0 * SIM_SCALE,
  80.     "B2":        6.6 * SIM_SCALE,
  81.     "B3":        3.59 * SIM_SCALE,
  82.     "B4":        5.84 * SIM_SCALE,
  83.     "B5":        10.04 * SIM_SCALE,
  84.     "B6_bar":    5.79 * SIM_SCALE,
  85.     "B7":        10.04 * SIM_SCALE,
  86.     "B6_angle":  12.8,
  87.     "B7_angle":  30.0,
  88.     "speed":     22,
  89. }
  90.  
  91. PRESET2_PARAMS = {
  92.     "crank":     18.6,
  93.     "B2":        40.5,
  94.     "B3":        25.1,
  95.     "B4":        52.2,
  96.     "B5":        75.4,
  97.     "B6_bar":    60.2,
  98.     "B7":        153.4,
  99.     "B6_angle":  0.0,
  100.     "B7_angle":  30.5,
  101.     "speed":     60.0,
  102. }
  103.  
  104. PRESET3_PARAMS = {
  105.     "crank":     21.0,
  106.     "B2":        46.2,
  107.     "B3":        25.1,
  108.     "B4":        52.2,
  109.     "B5":        80.9,
  110.     "B6_bar":    56.0,
  111.     "B7":        230.3,
  112.     "B6_angle":  12.8,
  113.     "B7_angle":  30.0,
  114.     "speed":     22.0,
  115. }
  116.  
  117. # Frame pivot offsets from crank center (patent values * SIM_SCALE)
  118. FRAME_LOW_X  = 6.6 * SIM_SCALE
  119. FRAME_LOW_Y  = 1.97 * SIM_SCALE
  120. FRAME_HIGH_X = 2.6 * SIM_SCALE
  121. FRAME_HIGH_Y = 6.9 * SIM_SCALE
  122.  
  123. SLIDERS = [
  124.     ("crank",    "Crank B0",   10,   42),
  125.     ("B2",       "B2 J1-J4",   25,   80),
  126.     ("B3",       "B3 J3-J4",   12,   50),
  127.     ("B4",       "B4 J4-J6",   20,   70),
  128.     ("B5",       "B5 J6-J7",   35,  120),
  129.     ("B6_bar",   "B6 J5-J7",   20,   80),
  130.     ("B7",       "B7 foot",    35,  400),
  131.     ("B6_angle", "B6 angle",    0,   45),
  132.     ("B7_angle", "B7 angle",    0,   60),
  133.     ("speed",    "Speed",        5,   60),
  134. ]
  135.  
  136. SLIDER_X     = 25
  137. SLIDER_W     = 185
  138. SLIDER_H     = 6
  139. SLIDER_START = 390
  140. SLIDER_STEP  = 32
  141.  
  142. params   = dict(PRESET3_PARAMS)  # RST3 is the primary working config
  143. dragging = None
  144.  
  145. font      = pygame.font.SysFont("monospace", 12)
  146. font_sm   = pygame.font.SysFont("monospace", 11)
  147. font_bold = pygame.font.SysFont("monospace", 14, bold=True)
  148.  
  149. TRACE_LEN   = 300
  150. trace_front = []
  151. trace_rear  = []
  152. show_traces = True
  153.  
  154. # Actual foot path: ring buffer collecting one leg's foot over one full crank rotation
  155. actual_foot_path = []
  156. actual_foot_angle_start = None
  157.  
  158. # Torque tracking
  159. torque_history = []       # instantaneous torque values (Nm)
  160. prev_foot_pos = None      # previous foot position for velocity calc
  161. prev_foot_vel = None      # previous velocity for acceleration calc
  162. max_torque_3cyc = 0.0     # max torque over last 3 cycles
  163.  
  164. # Save button flash
  165. save_flash_msg = ""
  166. save_flash_timer = 0
  167.  
  168. # Mapping from slider param key to linkage segment endpoint pairs for highlighting
  169. SLIDER_SEGMENTS = {
  170.     "crank":    [("O", "C")],
  171.     "B2":       [("C", "D")],
  172.     "B3":       [("P_low", "D")],
  173.     "B4":       [("D", "E")],
  174.     "B5":       [("E", "G")],
  175.     "B6_bar":   [("P_high", "G")],
  176.     "B7":       [("E", "F")],
  177.     "B6_angle": [("D", "E")],
  178.     "B7_angle": [("E", "F")],
  179.     "speed":    [],
  180. }
  181.  
  182.  
  183. # ============================================================
  184. # Circle-circle intersection
  185. # ============================================================
  186. def circle_intersect(p1, r1, p2, r2):
  187.     dx, dy = p2[0]-p1[0], p2[1]-p1[1]
  188.     d = math.hypot(dx, dy)
  189.     if d < 1e-9 or d > r1+r2+1e-6 or d < abs(r1-r2)-1e-6:
  190.         return None, None
  191.     a  = (r1*r1 - r2*r2 + d*d) / (2.0*d)
  192.     h  = math.sqrt(max(0.0, r1*r1 - a*a))
  193.     mx, my = p1[0] + a*dx/d, p1[1] + a*dy/d
  194.     return (mx+h*dy/d, my-h*dx/d), (mx-h*dy/d, my+h*dx/d)
  195.  
  196.  
  197. # ============================================================
  198. # Klann 6-bar solver
  199. # ============================================================
  200. def solve_klann(theta, p):
  201.     """
  202.    Solve Klann patent linkage.
  203.    Computation in math coords (Y up), output in screen coords (Y down).
  204.    """
  205.     # Fixed frame points (math coords, Y up)
  206.     O = (0.0, 0.0)
  207.     P_low = (-FRAME_LOW_X, -FRAME_LOW_Y)      # J3: left and below
  208.     P_high = (-FRAME_HIGH_X, FRAME_HIGH_Y)     # J5: left and above
  209.  
  210.     # J1: crank pin (clockwise in math coords)
  211.     C = (p["crank"] * math.cos(-theta),
  212.          p["crank"] * math.sin(-theta))
  213.  
  214.     # J4 (D): intersection of circle(J1, B2) and circle(J3, B3)
  215.     pts = circle_intersect(C, p["B2"], P_low, p["B3"])
  216.     if pts[0] is None:
  217.         return None
  218.     D = pts[0] if pts[0][1] > pts[1][1] else pts[1]  # high solution
  219.  
  220.     # J6 (E): lineextendBentDegrees(J4, J1, B4, -B6_angle)
  221.     # From J4, away from J1, bent by -B6_angle
  222.     slope_dc = math.atan2(C[1] - D[1], C[0] - D[0])
  223.     b6_rad = math.radians(-p["B6_angle"])
  224.     Ex = D[0] - p["B4"] * math.cos(slope_dc + b6_rad)
  225.     Ey = D[1] - p["B4"] * math.sin(slope_dc + b6_rad)
  226.     E = (Ex, Ey)
  227.  
  228.     # J7 (G): intersection of circle(J5, B6_bar) and circle(J6, B5)
  229.     pts2 = circle_intersect(P_high, p["B6_bar"], E, p["B5"])
  230.     if pts2[0] is None:
  231.         return None
  232.     G = pts2[0] if pts2[0][1] > pts2[1][1] else pts2[1]  # high solution
  233.  
  234.     # J8 (F, foot): lineextendBentDegrees(J6, J7, B7, B7_angle)
  235.     # From J6, away from J7, bent by B7_angle
  236.     slope_eg = math.atan2(G[1] - E[1], G[0] - E[0])
  237.     b7_rad = math.radians(p["B7_angle"])
  238.     Fx = E[0] - p["B7"] * math.cos(slope_eg + b7_rad)
  239.     Fy = E[1] - p["B7"] * math.sin(slope_eg + b7_rad)
  240.     F = (Fx, Fy)
  241.  
  242.     # Convert to screen coords (negate Y)
  243.     def s(pt):
  244.         return (pt[0], -pt[1])
  245.  
  246.     return dict(O=s(O), C=s(C), P_low=s(P_low), P_high=s(P_high),
  247.                 D=s(D), E=s(E), G=s(G), F=s(F))
  248.  
  249.  
  250. # ============================================================
  251. # Pre-compute foot path
  252. # ============================================================
  253. def compute_foot_path(p, steps=240):
  254.     path = []
  255.     for i in range(steps):
  256.         theta = 2.0 * math.pi * i / steps
  257.         j = solve_klann(theta, p)
  258.         if j is not None:
  259.             path.append(j["F"])
  260.     return path
  261.  
  262.  
  263. def path_metrics(path):
  264.     """Return (stride_px, height_px, min_x, max_x, min_y, max_y,
  265.            ground_contact_px, ground_points, foot_strike)."""
  266.     if not path:
  267.         return 0, 0, 0, 0, 0, 0, 0, [], False
  268.     xs = [p[0] for p in path]
  269.     ys = [p[1] for p in path]
  270.     max_y = max(ys)
  271.     height_px = max(ys) - min(ys)
  272.     tol = max(2.0, height_px * 0.02)
  273.     n = len(path)
  274.  
  275.     foot_strike = False
  276.     for i in range(n):
  277.         if path[i][1] >= max_y - tol:
  278.             dx = path[(i+1) % n][0] - path[i][0]
  279.             if dx > 0:
  280.                 foot_strike = True
  281.                 break
  282.  
  283.     ground_pts = [p for p in path if p[1] >= max_y - tol]
  284.     if ground_pts:
  285.         gxs = [p[0] for p in ground_pts]
  286.         ground_contact_px = max(gxs) - min(gxs)
  287.     else:
  288.         ground_contact_px = 0
  289.  
  290.     return (max(xs)-min(xs), height_px,
  291.             min(xs), max(xs), min(ys), max(ys),
  292.             ground_contact_px, ground_pts, foot_strike)
  293.  
  294.  
  295. # ============================================================
  296. # Save geometry as JSON for OpenSCAD fabrication
  297. # ============================================================
  298. def save_geometry(params, actual_path, max_torque):
  299.     """Export current linkage geometry as a JSON file for 3D printing.
  300.    All lengths in mm (param values = mm).
  301.    Each piece lists bearing hole positions (center-to-center for 624ZZ).
  302.    Uses the actual foot path (from simulation) for step profile.
  303.    """
  304.     ts = int(time.time())
  305.     crank = params["crank"]
  306.     B2 = params["B2"]
  307.     B3 = params["B3"]
  308.     B4 = params["B4"]
  309.     B5 = params["B5"]
  310.     B6 = params["B6_bar"]
  311.     B7 = params["B7"]
  312.     b6a = params["B6_angle"]
  313.     b7a = params["B7_angle"]
  314.     b6r = math.radians(b6a)
  315.     b7r = math.radians(b7a)
  316.  
  317.     # Frame L-bracket: J0 at origin, arms to J3 and J5 (math coords, Y up)
  318.     frame_j3 = (-FRAME_LOW_X, -FRAME_LOW_Y)
  319.     frame_j5 = (-FRAME_HIGH_X, FRAME_HIGH_Y)
  320.     frame_arm1_len = math.hypot(FRAME_LOW_X, FRAME_LOW_Y)
  321.     frame_arm2_len = math.hypot(FRAME_HIGH_X, FRAME_HIGH_Y)
  322.     a1 = math.atan2(frame_j3[1], frame_j3[0])
  323.     a2 = math.atan2(frame_j5[1], frame_j5[0])
  324.     frame_opening_deg = math.degrees(a2 - a1) % 360
  325.  
  326.     # Bent coupler (b2+b4): 3 holes at J1, J4 (vertex), J6
  327.     # Relative to J4 with J1 along +X
  328.     coupler_j1 = (B2, 0.0)
  329.     coupler_j6 = (-B4 * math.cos(b6r), B4 * math.sin(b6r))
  330.  
  331.     # Bent output (b5+b7): 3 holes at J7, J6 (vertex), J8
  332.     # Relative to J6 with J7 along +X
  333.     output_j7 = (B5, 0.0)
  334.     output_j8 = (-B7 * math.cos(b7r), -B7 * math.sin(b7r))
  335.  
  336.     def r2(v):
  337.         return round(v, 2)
  338.  
  339.     # 6 manufactured pieces: frame + 3 simple bars + 2 bent bars
  340.     #
  341.     # Assembly topology:
  342.     #   J0 (frame) ── b0 ── J1 (crank pin)
  343.     #   J1 ── [b2+b4] ── J4 ── J6   (single bent piece, vertex at J4)
  344.     #   J3 (frame) ── b3 ── J4
  345.     #   J6 ── [b5+b7] ── J7 / J8    (single bent piece, vertex at J6)
  346.     #   J5 (frame) ── b6 ── J7
  347.  
  348.     data = {
  349.         "timestamp": ts,
  350.         "units": "mm",
  351.         "bearing": {
  352.             "model": "624ZZ",
  353.             "od_mm": 13.0,
  354.             "id_mm": 4.0,
  355.             "width_mm": 5.0,
  356.         },
  357.         "parameters": {
  358.             "crank_mm": r2(crank),
  359.             "B2_mm": r2(B2),
  360.             "B3_mm": r2(B3),
  361.             "B4_mm": r2(B4),
  362.             "B5_mm": r2(B5),
  363.             "B6_bar_mm": r2(B6),
  364.             "B7_mm": r2(B7),
  365.             "B6_angle_deg": r2(b6a),
  366.             "B7_angle_deg": r2(b7a),
  367.             "frame_low_x_mm": r2(FRAME_LOW_X),
  368.             "frame_low_y_mm": r2(FRAME_LOW_Y),
  369.             "frame_high_x_mm": r2(FRAME_HIGH_X),
  370.             "frame_high_y_mm": r2(FRAME_HIGH_Y),
  371.         },
  372.         "pieces": [
  373.             {
  374.                 "name": "frame",
  375.                 "description": "Fixed L-bracket. Motor shaft at J0, two pivot holes.",
  376.                 "type": "frame",
  377.                 "holes": [
  378.                     {"label": "J0", "x_mm": 0, "y_mm": 0,
  379.                      "note": "crank motor shaft"},
  380.                     {"label": "J3", "x_mm": r2(frame_j3[0]), "y_mm": r2(frame_j3[1]),
  381.                      "note": "lower pivot"},
  382.                     {"label": "J5", "x_mm": r2(frame_j5[0]), "y_mm": r2(frame_j5[1]),
  383.                      "note": "upper pivot"},
  384.                 ],
  385.                 "arm1_length_mm": r2(frame_arm1_len),
  386.                 "arm1_endpoint": "J3",
  387.                 "arm2_length_mm": r2(frame_arm2_len),
  388.                 "arm2_endpoint": "J5",
  389.                 "opening_angle_deg": r2(frame_opening_deg),
  390.             },
  391.             {
  392.                 "name": "b0_crank",
  393.                 "description": "Crank arm. Pivot to crank pin.",
  394.                 "type": "bar",
  395.                 "joints": ["J0", "J1"],
  396.                 "center_to_center_mm": r2(crank),
  397.             },
  398.             {
  399.                 "name": "b2_b4_coupler",
  400.                 "description": "Bent coupler: J1--J4--J6. Single rigid piece, "
  401.                                "vertex at J4. B2 arm to crank pin, B4 arm to output.",
  402.                 "type": "bent_bar",
  403.                 "vertex": "J4",
  404.                 "joints": ["J1", "J4", "J6"],
  405.                 "holes": [
  406.                     {"label": "J1", "x_mm": r2(coupler_j1[0]),
  407.                      "y_mm": r2(coupler_j1[1]), "note": "crank pin"},
  408.                     {"label": "J4", "x_mm": 0, "y_mm": 0,
  409.                      "note": "vertex, pivots on b3 rocker"},
  410.                     {"label": "J6", "x_mm": r2(coupler_j6[0]),
  411.                      "y_mm": r2(coupler_j6[1]), "note": "output node"},
  412.                 ],
  413.                 "arm1_length_mm": r2(B2),
  414.                 "arm2_length_mm": r2(B4),
  415.                 "vertex_angle_deg": r2(180 - b6a),
  416.             },
  417.             {
  418.                 "name": "b3_rocker",
  419.                 "description": "Lower rocker. Frame pivot J3 to coupler node J4.",
  420.                 "type": "bar",
  421.                 "joints": ["J3", "J4"],
  422.                 "center_to_center_mm": r2(B3),
  423.             },
  424.             {
  425.                 "name": "b5_b7_output",
  426.                 "description": "Bent output: J7--J6--J8. Single rigid piece, "
  427.                                "vertex at J6. B5 arm to upper rocker, B7 arm is foot.",
  428.                 "type": "bent_bar",
  429.                 "vertex": "J6",
  430.                 "joints": ["J7", "J6", "J8"],
  431.                 "holes": [
  432.                     {"label": "J7", "x_mm": r2(output_j7[0]),
  433.                      "y_mm": r2(output_j7[1]), "note": "upper rocker"},
  434.                     {"label": "J6", "x_mm": 0, "y_mm": 0,
  435.                      "note": "vertex, pivots on coupler"},
  436.                     {"label": "J8", "x_mm": r2(output_j8[0]),
  437.                      "y_mm": r2(output_j8[1]), "note": "foot"},
  438.                 ],
  439.                 "arm1_length_mm": r2(B5),
  440.                 "arm2_length_mm": r2(B7),
  441.                 "vertex_angle_deg": r2(180 + b7a),
  442.             },
  443.             {
  444.                 "name": "b6_rocker",
  445.                 "description": "Upper rocker. Frame pivot J5 to triangle apex J7.",
  446.                 "type": "bar",
  447.                 "joints": ["J5", "J7"],
  448.                 "center_to_center_mm": r2(B6),
  449.             },
  450.         ],
  451.     }
  452.  
  453.     # Use actual foot path from simulation for step profile
  454.     # actual_path is in screen coords (absolute), need relative positions
  455.     # Extract the foot's path relative to hip (subtract the constant hip offset)
  456.     # The actual_path has Y near GROUND_Y when on ground — use it directly for metrics
  457.     if actual_path and len(actual_path) > 10:
  458.         foot_path = list(actual_path)
  459.     else:
  460.         # Fallback to computed if no actual data yet
  461.         linkage_p = {k: v for k, v in params.items() if k != "speed"}
  462.         foot_path = compute_foot_path(linkage_p)
  463.  
  464.     stride_px, height_px, mn_x, mx_x, mn_y, mx_y, ground_px, ground_pts, _ = path_metrics(foot_path)
  465.     step_height_mm = height_px
  466.     step_length_mm = ground_px if ground_px > 0 else stride_px
  467.  
  468.     data["step_profile"] = {
  469.         "step_length_mm": r2(step_length_mm),
  470.         "step_height_mm": r2(step_height_mm),
  471.         "stride_width_mm": r2(stride_px),
  472.         "max_torque_3cyc_Nm": r2(max_torque),
  473.         "recommended_torque_Nm": r2(max_torque * 2.85),
  474.     }
  475.  
  476.     script_dir = os.path.dirname(os.path.abspath(__file__))
  477.     filepath = os.path.join(script_dir, f"{ts}.json")
  478.     with open(filepath, 'w') as f:
  479.         json.dump(data, f, indent=2)
  480.  
  481.     # --- Generate PNG of step profile ---
  482.     png_path = os.path.join(script_dir, f"{ts}.png")
  483.     _save_profile_png(png_path, foot_path, ground_pts,
  484.                       step_length_mm, step_height_mm, stride_px,
  485.                       mn_x, mx_x, mn_y, mx_y, params, ts, max_torque)
  486.  
  487.     # --- Generate per-piece OpenSCAD + STL files ---
  488.     _save_scad_and_stl(script_dir, params, ts)
  489.  
  490.     return filepath
  491.  
  492.  
  493. # Shared SCAD preamble for all pieces
  494. def _stl_to_step(stl_path):
  495.     """Convert an STL file to STEP format using OCP (OpenCascade via CadQuery)."""
  496.     step_path = stl_path.replace('.stl', '.step')
  497.     try:
  498.         from OCP.STEPControl import STEPControl_Writer, STEPControl_AsIs
  499.         from OCP.StlAPI import StlAPI_Reader
  500.         from OCP.TopoDS import TopoDS_Shape
  501.  
  502.         reader = StlAPI_Reader()
  503.         shape = TopoDS_Shape()
  504.         reader.Read(shape, stl_path)
  505.         if shape.IsNull():
  506.             return None
  507.  
  508.         writer = STEPControl_Writer()
  509.         writer.Transfer(shape, STEPControl_AsIs)
  510.         writer.Write(step_path)
  511.         if os.path.exists(step_path):
  512.             return step_path
  513.     except Exception as e:
  514.         print(f"STEP export failed for {os.path.basename(stl_path)}: {e}")
  515.     return None
  516.  
  517.  
  518. _SCAD_PREAMBLE = """\
  519. // 624ZZ bearing dimensions
  520. BEARING_OD     = 13.0;
  521. BEARING_ID     =  4.0;
  522. BEARING_W      =  5.0;
  523. BOLT_CLEARANCE =  4.3;   // M4 + 0.3mm clearance
  524.  
  525. // Link geometry
  526. LINK_THICK     = 10.0;
  527. WALL           =  3.0;
  528. LINK_WIDTH     = BEARING_OD + 2 * WALL;  // 19mm
  529. POCKET_DEPTH   =  5.0;
  530. POCKET_DIA     = 13.4;
  531.  
  532. // Captured M4 nut recess (bottom face of each bearing)
  533. NUT_RECESS_DEPTH = 3.5;
  534. NUT_RECESS_DIA   = 8.66;  // 7.5mm AF / cos(30) for $fn=6
  535.  
  536. module bearing_seat(thick=LINK_THICK) {
  537.    // Bolt hole through full thickness
  538.    translate([0, 0, -0.05])
  539.        cylinder(h = thick + 0.1, d = BOLT_CLEARANCE, $fn = 24);
  540.    // Bearing pocket on top face
  541.    translate([0, 0, thick - POCKET_DEPTH])
  542.        cylinder(h = POCKET_DEPTH + 0.05, d = POCKET_DIA, $fn = 48);
  543.    // Captured nut recess on bottom face
  544.    translate([0, 0, -0.05])
  545.        cylinder(h = NUT_RECESS_DEPTH + 0.05, d = NUT_RECESS_DIA, $fn = 6);
  546. }
  547.  
  548. // Bearing seat that also flattens any ridge/pyramid material above it.
  549. module bearing_seat_flat(base_thick=LINK_THICK, max_thick=12) {
  550.    // Bolt hole through full height
  551.    translate([0, 0, -0.05])
  552.        cylinder(h = max_thick + 0.1, d = BOLT_CLEARANCE, $fn = 24);
  553.    // Bearing pocket at base height
  554.    translate([0, 0, base_thick - POCKET_DEPTH])
  555.        cylinder(h = POCKET_DEPTH + 0.05, d = POCKET_DIA, $fn = 48);
  556.    // Captured nut recess on bottom face
  557.    translate([0, 0, -0.05])
  558.        cylinder(h = NUT_RECESS_DEPTH + 0.05, d = NUT_RECESS_DIA, $fn = 6);
  559.    // Flatten: remove all ridge material above the bearing hub
  560.    translate([0, 0, base_thick - 0.01])
  561.        cylinder(h = max_thick, d = LINK_WIDTH, $fn = 48);
  562. }
  563.  
  564. module straight_bar(c2c) {
  565.    difference() {
  566.        hull() {
  567.            cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  568.            translate([c2c, 0, 0])
  569.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  570.        }
  571.        bearing_seat();
  572.        translate([c2c, 0, 0]) bearing_seat();
  573.    }
  574. }
  575.  
  576. // Bent bar: two straight arms joined at vertex (boomerang/L shape)
  577. // h1x,h1y = first endpoint, h2x,h2y = second endpoint, vertex at (0,0)
  578. module bent_bar(h1x, h1y, h2x, h2y) {
  579.    difference() {
  580.        union() {
  581.            // Arm 1: vertex to endpoint 1
  582.            hull() {
  583.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  584.                translate([h1x, h1y, 0])
  585.                    cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  586.            }
  587.            // Arm 2: vertex to endpoint 2
  588.            hull() {
  589.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  590.                translate([h2x, h2y, 0])
  591.                    cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  592.            }
  593.        }
  594.        // Vertex bearing seat
  595.        bearing_seat();
  596.        // Endpoint 1 bearing seat
  597.        translate([h1x, h1y, 0]) bearing_seat();
  598.        // Endpoint 2 bearing seat
  599.        translate([h2x, h2y, 0]) bearing_seat();
  600.    }
  601. }
  602. """
  603.  
  604.  
  605. def _save_scad_and_stl(out_dir, params, ts):
  606.     """Generate per-piece .scad and .stl files."""
  607.     import subprocess
  608.  
  609.     crank = params["crank"]
  610.     B2 = params["B2"]
  611.     B3 = params["B3"]
  612.     B4 = params["B4"]
  613.     B5 = params["B5"]
  614.     B6 = params["B6_bar"]
  615.     B7 = params["B7"]
  616.     b6r = math.radians(params["B6_angle"])
  617.     b7r = math.radians(params["B7_angle"])
  618.  
  619.     # Bent coupler hole positions (relative to vertex J4, J1 along +X)
  620.     c_j1x, c_j1y = B2, 0.0
  621.     c_j6x = -B4 * math.cos(b6r)
  622.     c_j6y =  B4 * math.sin(b6r)
  623.  
  624.     # Bent output hole positions (relative to vertex J6, J7 along +X)
  625.     o_j7x, o_j7y = B5, 0.0
  626.     o_j8x = -B7 * math.cos(b7r)
  627.     o_j8y = -B7 * math.sin(b7r)
  628.  
  629.     # Find OpenSCAD binary
  630.     import shutil
  631.     openscad = shutil.which("openscad")
  632.     if openscad is None:
  633.         # macOS app bundle fallback
  634.         for p in ["/Applications/OpenSCAD.app/Contents/MacOS/OpenSCAD"]:
  635.             if os.path.isfile(p):
  636.                 openscad = p
  637.                 break
  638.     if openscad is None:
  639.         print("WARNING: openscad not found in PATH. .scad files saved but .stl skipped.")
  640.  
  641.     # Frame bracket: flat-bottomed shape with mounting holes
  642.     j3x = -FRAME_LOW_X
  643.     j3y = -FRAME_LOW_Y
  644.     j5x = -FRAME_HIGH_X
  645.     j5y = FRAME_HIGH_Y
  646.     hub_d = 13.0 + 2 * 3.0  # 19mm: bearing OD + 3mm wall each side
  647.  
  648.     # Mounting holes: below J0 (hole A), for bolting to chassis
  649.     # Mount 1: directly below J0 at (0, mount_y)
  650.     # Mount 2: 25mm to the left at (-25, mount_y)
  651.     mount_y = -20.0           # 20mm below J0
  652.     mount1_x = 0.0            # directly below J0
  653.     mount2_x = -25.0          # 25mm in front (left)
  654.     nut_af = 7.5              # M4 hex nut: 7mm AF + 0.5mm clearance
  655.     nut_depth = 3.2           # M4 nut: 3.0mm thick + 0.2mm clearance
  656.     nut_ac = nut_af / math.cos(math.radians(30))  # across corners ~7.5mm
  657.     nut_r = nut_ac / 2        # corner radius ~3.75mm
  658.     wall_clearance = 3.0      # min PLA from nut edge to part edge
  659.  
  660.     # Bottom edge: must clear below the nut recesses
  661.     bot_y = mount_y - nut_r - wall_clearance  # ~-26.75
  662.     # Bottom corners extend to cover full width + mount holes
  663.     bot_left_x = min(j3x, j5x, mount2_x) - max(hub_d / 2, nut_r + wall_clearance)
  664.     bot_right_x = max(j3x, j5x, mount1_x) + max(hub_d / 2, nut_r + wall_clearance)
  665.  
  666.     frame_scad = _SCAD_PREAMBLE + f"""\
  667. FRAME_THICK = 10.0;
  668. HUB_D = {hub_d:.1f};
  669.  
  670. module frame_bracket() {{
  671.    difference() {{
  672.        hull() {{
  673.            // Bearing hubs (19mm circles guarantee 3mm wall around pockets)
  674.            cylinder(h = FRAME_THICK, d = HUB_D, $fn = 48);
  675.            translate([{j3x:.2f}, {j3y:.2f}, 0])
  676.                cylinder(h = FRAME_THICK, d = HUB_D, $fn = 48);
  677.            translate([{j5x:.2f}, {j5y:.2f}, 0])
  678.                cylinder(h = FRAME_THICK, d = HUB_D, $fn = 48);
  679.            // Flat bottom edge (extends below for mounting area)
  680.            translate([{bot_left_x:.2f}, {bot_y:.2f}, 0])
  681.                cylinder(h = FRAME_THICK, d = 1, $fn = 8);
  682.            translate([{bot_right_x:.2f}, {bot_y:.2f}, 0])
  683.                cylinder(h = FRAME_THICK, d = 1, $fn = 8);
  684.        }}
  685.        // J0: bearing seat
  686.        bearing_seat(FRAME_THICK);
  687.        // J3: bearing seat
  688.        translate([{j3x:.2f}, {j3y:.2f}, 0]) bearing_seat(FRAME_THICK);
  689.        // J5: bearing seat
  690.        translate([{j5x:.2f}, {j5y:.2f}, 0]) bearing_seat(FRAME_THICK);
  691.        // Mounting hole 1: directly below J0 (hole A)
  692.        translate([{mount1_x:.2f}, {mount_y:.2f}, -0.05])
  693.            cylinder(h = FRAME_THICK + 0.1, d = BOLT_CLEARANCE, $fn = 24);
  694.        // Mounting hole 2: 25mm to the left
  695.        translate([{mount2_x:.2f}, {mount_y:.2f}, -0.05])
  696.            cylinder(h = FRAME_THICK + 0.1, d = BOLT_CLEARANCE, $fn = 24);
  697.        // Hex nut recess 1 (top face, same side as bearing pockets)
  698.        // d = across-corners so $fn=6 gives correct across-flats = {nut_af:.1f}mm
  699.        translate([{mount1_x:.2f}, {mount_y:.2f}, FRAME_THICK - {nut_depth}])
  700.            cylinder(h = {nut_depth + 0.05}, d = {nut_ac:.2f}, $fn = 6);
  701.        // Hex nut recess 2
  702.        translate([{mount2_x:.2f}, {mount_y:.2f}, FRAME_THICK - {nut_depth}])
  703.            cylinder(h = {nut_depth + 0.05}, d = {nut_ac:.2f}, $fn = 6);
  704.    }}
  705. }}
  706.  
  707. frame_bracket();
  708. """
  709.  
  710.     # Build b5_b7 output piece(s) — split if B7 arm >= 105mm
  711.     SPLIT_THRESHOLD = 105.0
  712.     CUT_DIST = 40.0        # mm from J6 along B7 arm where the cut happens
  713.     PIN_DIA = 1.95         # 1.75mm filament + 0.2mm clearance
  714.     PIN_DEPTH = 10.5       # slightly over 10mm so pins seat fully
  715.     # 3 alignment pins in equilateral triangle (circumradius R=3mm)
  716.     PIN_R = 3.0            # circumradius of equilateral triangle
  717.     # Pin positions relative to face center (Y=across bar, Z=thickness)
  718.     # Top pin, bottom-left, bottom-right
  719.     PIN_TRI = [
  720.         (0.0, PIN_R),                                       # top center
  721.         (-PIN_R * math.sin(math.radians(60)), -PIN_R * 0.5), # bottom-left
  722.         ( PIN_R * math.sin(math.radians(60)), -PIN_R * 0.5), # bottom-right
  723.     ]
  724.  
  725.     # Pyramidal ridge on B7 arm (Z axis) for torsion resistance
  726.     # Ridge must start 17.5mm from EDGE of bearing well, not center
  727.     # bearing well radius = POCKET_DIA/2 = 6.7mm
  728.     # Bottom stays flat at z=0 for bed adhesion; ridge is on top only
  729.     RIDGE_H = 16.0        # mm total height at peak (base + 6mm ridge)
  730.     RIDGE_W = 6.0         # mm width of ridge spine at peak
  731.     BEARING_WELL_R = 13.4 / 2  # pocket radius
  732.     RIDGE_CLEARANCE = 17.5 + BEARING_WELL_R  # ~24.2mm from hole center
  733.     b7_len_full = math.hypot(o_j8x, o_j8y)
  734.  
  735.     if B7 >= SPLIT_THRESHOLD:
  736.         # Split the B7 arm into elbow + extension
  737.         b7_len = b7_len_full
  738.         dx = o_j8x / b7_len
  739.         dy = o_j8y / b7_len
  740.         # Perpendicular for pin placement
  741.         px, py = -dy, dx
  742.         # Cut point
  743.         cx, cy = dx * CUT_DIST, dy * CUT_DIST
  744.         # Pin holes: 3 pins in equilateral triangle at the cut face
  745.         # Perpendicular (across bar) = (px, py), thickness = Z axis
  746.         # Compute 3 pin start positions (1mm outside cut face for clean drilling)
  747.         pin_start_offset = 1.0
  748.         arm_ang = math.degrees(math.atan2(dy, dx))
  749.         pin_total = PIN_DEPTH + pin_start_offset
  750.         # Each pin: face_center + perp_offset * (px,py) + z_offset in Z
  751.         elbow_pin_positions = []
  752.         ext_pin_positions = []
  753.         for (perp_off, z_off) in PIN_TRI:
  754.             # Elbow: start 1mm outside cut face (toward J8)
  755.             epx = cx + dx * pin_start_offset + perp_off * px
  756.             epy = cy + dy * pin_start_offset + perp_off * py
  757.             epz = 5.0 + z_off  # LINK_THICK/2 = 5
  758.             elbow_pin_positions.append((epx, epy, epz))
  759.             # Extension: at x=-1 (1mm outside), Y=perp_off, Z=5+z_off
  760.             ext_pin_positions.append((perp_off, epz))
  761.         # Extension length
  762.         ext_len = b7_len - CUT_DIST
  763.  
  764.         # Elbow: full bent bar, then intersect to chop flat at cut line
  765.         # The cut plane passes through cut_point, perpendicular to the B7 arm
  766.         # We keep the J6/J7 side. Use a large cube on the "keep" side.
  767.         # Normal of cut plane points toward J8: (dx, dy). We keep the opposite side.
  768.         # Rotate cut cube so its face aligns with the cut perpendicular.
  769.         elbow_scad = _SCAD_PREAMBLE + f"""\
  770. // b5_b7 elbow: bent bar chopped flat at {CUT_DIST:.0f}mm along B7 arm
  771. // No pyramid on elbow (upper section) — only on extension (lower section)
  772. difference() {{
  773.    intersection() {{
  774.        union() {{
  775.            // Arm 1: vertex (J6) to J7
  776.            hull() {{
  777.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  778.                translate([{o_j7x:.2f}, {o_j7y:.2f}, 0])
  779.                    cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  780.            }}
  781.            // Arm 2: vertex (J6) toward J8 (flat, no ridge)
  782.            hull() {{
  783.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  784.                translate([{o_j8x:.2f}, {o_j8y:.2f}, 0])
  785.                    cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  786.            }}
  787.        }}
  788.        // Half-space: keep everything on J6 side of cut plane
  789.        translate([{cx:.2f}, {cy:.2f}, -1])
  790.            rotate([0, 0, {arm_ang:.2f}])
  791.            translate([-500, -500, 0])
  792.            cube([500, 1000, LINK_THICK + 2]);
  793.    }}
  794.    // J6 bearing seat (vertex)
  795.    bearing_seat();
  796.    // J7 bearing seat
  797.    translate([{o_j7x:.2f}, {o_j7y:.2f}, 0]) bearing_seat();
  798.    // 3 alignment pin holes in equilateral triangle at cut face
  799. {"".join(f'''    translate([{epx:.2f}, {epy:.2f}, {epz:.2f}])
  800.        rotate([0, 0, {arm_ang + 180:.2f}])
  801.        rotate([0, 90, 0])
  802.        cylinder(h = {pin_total:.1f}, d = {PIN_DIA}, $fn = 16);
  803. ''' for epx, epy, epz in elbow_pin_positions)}
  804. }}
  805. """
  806.  
  807.         # Extension: full straight bar (cut_point to J8), chopped flat at cut end
  808.         # Oriented along X: x=0 is cut face, x=ext_len is J8
  809.         # Ridge on extension: 17.5mm clearance from cut face (x=0) and J8 (x=ext_len)
  810.         ridge_start_x = RIDGE_CLEARANCE          # 17.5mm from cut face
  811.         ridge_end_x = ext_len - RIDGE_CLEARANCE   # 17.5mm from J8
  812.         ridge_mid_x = (ridge_start_x + ridge_end_x) / 2
  813.         has_ridge = ridge_end_x > ridge_start_x + 10  # only if enough room
  814.  
  815.         if has_ridge:
  816.             ext_hull = f"""
  817.        // Bar with pyramidal ridge, 17.5mm clearance from each hole
  818.        hull() {{
  819.            cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  820.            translate([{ext_len:.2f}, 0, 0])
  821.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  822.            // Ridge start (17.5mm from cut face)
  823.            translate([{ridge_start_x:.2f}, 0, 0])
  824.                cylinder(h = {RIDGE_H}, d = {RIDGE_W}, $fn = 24);
  825.            // Ridge end (17.5mm from J8)
  826.            translate([{ridge_end_x:.2f}, 0, 0])
  827.                cylinder(h = {RIDGE_H}, d = {RIDGE_W}, $fn = 24);
  828.        }}"""
  829.         else:
  830.             ext_hull = f"""
  831.        // Bar too short for ridge, flat
  832.        hull() {{
  833.            cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  834.            translate([{ext_len:.2f}, 0, 0])
  835.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  836.        }}"""
  837.  
  838.         ext_scad = _SCAD_PREAMBLE + f"""\
  839. // b5_b7 extension: cut_point to J8 ({ext_len:.1f}mm)
  840. // x=0 is flat cut face, x={ext_len:.2f} is J8
  841. // Ridge clearance: {RIDGE_CLEARANCE}mm from each hole
  842. difference() {{
  843.    intersection() {{
  844. {ext_hull}
  845.        // Chop: keep only x >= 0 (flat face at cut end)
  846.        translate([0, -LINK_WIDTH * 2, -1])
  847.            cube([{ext_len + 20:.2f}, LINK_WIDTH * 4, {RIDGE_H + 2}]);
  848.    }}
  849.    // J8 bearing seat (flatten ridge above)
  850.    translate([{ext_len:.2f}, 0, 0]) bearing_seat_flat(LINK_THICK, {RIDGE_H});
  851.    // 3 alignment pin holes in equilateral triangle at cut face
  852. {"".join(f'''    translate([-1, {y_off:.2f}, {z_off:.2f}])
  853.        rotate([0, 90, 0])
  854.        cylinder(h = {pin_total:.1f}, d = {PIN_DIA}, $fn = 16);
  855. ''' for y_off, z_off in ext_pin_positions)}
  856. }}
  857. """
  858.         b5_b7_pieces = [
  859.             ("b5_b7_elbow", elbow_scad),
  860.             ("b5_b7_ext",   ext_scad),
  861.         ]
  862.     else:
  863.         # Non-split: single bent bar with pyramidal ridge on B7 arm
  864.         # Ridge starts 17.5mm from J6 and ends 17.5mm from J8
  865.         b7_dx_ns = o_j8x / b7_len_full
  866.         b7_dy_ns = o_j8y / b7_len_full
  867.         rs_x = b7_dx_ns * RIDGE_CLEARANCE  # ridge start (17.5mm from J6)
  868.         rs_y = b7_dy_ns * RIDGE_CLEARANCE
  869.         re_x = o_j8x - b7_dx_ns * RIDGE_CLEARANCE  # ridge end (17.5mm from J8)
  870.         re_y = o_j8y - b7_dy_ns * RIDGE_CLEARANCE
  871.         ns_has_ridge = b7_len_full > 2 * RIDGE_CLEARANCE + 10
  872.  
  873.         if ns_has_ridge:
  874.             ns_ridge = f"""            // Ridge with {RIDGE_CLEARANCE}mm clearance from J6 and J8
  875.            translate([{rs_x:.2f}, {rs_y:.2f}, 0])
  876.                cylinder(h = {RIDGE_H}, d = {RIDGE_W}, $fn = 24);
  877.            translate([{re_x:.2f}, {re_y:.2f}, 0])
  878.                cylinder(h = {RIDGE_H}, d = {RIDGE_W}, $fn = 24);"""
  879.         else:
  880.             ns_ridge = ""
  881.  
  882.         b5_b7_scad = _SCAD_PREAMBLE + f"""\
  883. difference() {{
  884.    union() {{
  885.        // Arm 1: vertex (J6) to J7
  886.        hull() {{
  887.            cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  888.            translate([{o_j7x:.2f}, {o_j7y:.2f}, 0])
  889.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  890.        }}
  891.        // Arm 2: vertex (J6) to J8 with pyramidal ridge
  892.        hull() {{
  893.            cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  894.            translate([{o_j8x:.2f}, {o_j8y:.2f}, 0])
  895.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  896. {ns_ridge}
  897.        }}
  898.    }}
  899.    bearing_seat_flat(LINK_THICK, {RIDGE_H});
  900.    translate([{o_j7x:.2f}, {o_j7y:.2f}, 0]) bearing_seat();
  901.    translate([{o_j8x:.2f}, {o_j8y:.2f}, 0]) bearing_seat_flat(LINK_THICK, {RIDGE_H});
  902. }}
  903. """
  904.         b5_b7_pieces = [("b5_b7_output", b5_b7_scad)]
  905.  
  906.     pieces = [
  907.         ("frame",       frame_scad),
  908.         ("b0_crank",    _SCAD_PREAMBLE + f"straight_bar({crank:.2f});"),
  909.         ("b2_b4_coupler", _SCAD_PREAMBLE +
  910.             f"bent_bar({c_j1x:.2f}, {c_j1y:.2f}, {c_j6x:.2f}, {c_j6y:.2f});"),
  911.         ("b3_rocker",   _SCAD_PREAMBLE + f"straight_bar({B3:.2f});"),
  912.         ("b6_rocker",   _SCAD_PREAMBLE + f"straight_bar({B6:.2f});"),
  913.     ] + b5_b7_pieces
  914.  
  915.     for name, scad_content in pieces:
  916.         scad_path = os.path.join(out_dir, f"{ts}_{name}.scad")
  917.         stl_path  = os.path.join(out_dir, f"{ts}_{name}.stl")
  918.  
  919.         with open(scad_path, 'w') as f:
  920.             f.write(scad_content)
  921.  
  922.         if openscad:
  923.             try:
  924.                 result = subprocess.run(
  925.                     [openscad, "-o", stl_path, scad_path],
  926.                     timeout=120, capture_output=True, text=True
  927.                 )
  928.                 if result.returncode != 0:
  929.                     print(f"OpenSCAD error ({name}): {result.stderr[:200]}")
  930.                 elif os.path.exists(stl_path):
  931.                     _stl_to_step(stl_path)
  932.             except FileNotFoundError:
  933.                 print(f"OpenSCAD not found at: {openscad}")
  934.                 openscad = None  # stop trying for remaining pieces
  935.             except Exception as e:
  936.                 print(f"OpenSCAD failed ({name}): {e}")
  937.         elif not openscad:
  938.             pass  # already warned or not installed
  939.  
  940.  
  941. # ============================================================
  942. # Servo test box — Feetech ST3215-C047
  943. # ============================================================
  944. # Servo: 45.4 x 24.7 x 35.0 mm (measured), shaft 2.15mm coupling hole
  945. # Stall torque: 30 kg.cm (2.94 Nm) @ 12V, gear ratio 1:345
  946. # TTL serial half-duplex, ~73g
  947. _SERVO_BOX_SCAD = """\
  948. // Servo Test Box — Feetech ST3215-C047
  949. // 5 solid sides (open top only), 10mm vent on each side
  950. SERVO_L=45.12; SERVO_W=24.45; SERVO_H=35.0; SHAFT_DIA=2.15;
  951. WALL=3; CL=0.4; VENT=21;
  952. CL_L=SERVO_L+CL; CL_W=SERVO_W+CL; CL_H=SERVO_H+CL;
  953. BL=CL_L+2*WALL; BW=CL_W+2*WALL; BH=CL_H+WALL;
  954. difference() {
  955.    cube([BL, BW, BH]);
  956.    // Cavity (open top only for slide-in)
  957.    translate([WALL,WALL,WALL]) cube([CL_L,CL_W,CL_H+1]);
  958.    // Shaft hole: 22mm dia, 10mm from inside edge, centered in width
  959.    translate([WALL+10.0,WALL+CL_W/2,-0.1])
  960.        cylinder(h=WALL+0.2, d=22, $fn=48);
  961.    // Bottom vent: centered on bottom face
  962.    translate([BL/2,BW/2,-0.1])
  963.        cylinder(h=WALL+0.2, d=VENT, $fn=32);
  964.    // 4 wall vents: drill along X for left/right, along Y for front/back
  965.    // Left wall (X=0)
  966.    translate([-0.1, BW/2, WALL+CL_H/2]) rotate([0,90,0])
  967.        cylinder(h=WALL+0.2, d=VENT, $fn=32);
  968.    // Right wall (X=BL)
  969.    translate([BL+0.1, BW/2, WALL+CL_H/2]) rotate([0,-90,0])
  970.        cylinder(h=WALL+0.2, d=VENT, $fn=32);
  971.    // Front wall (Y=BW)
  972.    translate([BL/2, BW+0.1, WALL+CL_H/2]) rotate([90,0,0])
  973.        cylinder(h=WALL+0.2, d=VENT, $fn=32);
  974.    // Back wall (Y=0)
  975.    translate([BL/2, -0.1, WALL+CL_H/2]) rotate([-90,0,0])
  976.        cylinder(h=WALL+0.2, d=VENT, $fn=32);
  977. }
  978. """
  979.  
  980. def _save_servo_box():
  981.     import subprocess, shutil
  982.     script_dir = os.path.dirname(os.path.abspath(__file__))
  983.     ts = int(time.time())
  984.     scad_path = os.path.join(script_dir, f"{ts}_servo_box.scad")
  985.     stl_path = os.path.join(script_dir, f"{ts}_servo_box.stl")
  986.     with open(scad_path, 'w') as f:
  987.         f.write(_SERVO_BOX_SCAD)
  988.     openscad = shutil.which("openscad")
  989.     if not openscad:
  990.         for p in ["/Applications/OpenSCAD.app/Contents/MacOS/OpenSCAD"]:
  991.             if os.path.isfile(p):
  992.                 openscad = p; break
  993.     if openscad:
  994.         try:
  995.             subprocess.run([openscad, "-o", stl_path, scad_path],
  996.                            timeout=120, capture_output=True, text=True)
  997.             if os.path.exists(stl_path):
  998.                 _stl_to_step(stl_path)
  999.         except Exception:
  1000.             pass
  1001.     return scad_path, stl_path
  1002.  
  1003.  
  1004. def _save_frame_joined():
  1005.     """Generate frame + servo box as a single joined piece.
  1006.    The servo box is attached behind the frame (Z<0 side),
  1007.    with the servo output shaft concentric with J0 (hole A).
  1008.    The frame is extended with gentle arcs to cover the servo box.
  1009.    """
  1010.     import subprocess, shutil
  1011.  
  1012.     # --- Servo box dimensions ---
  1013.     SERVO_L = 45.12; SERVO_W = 24.45; SERVO_H = 35.0
  1014.     S_WALL = 3.0; S_CL = 0.4; VENT = 21.0
  1015.     CL_L = SERVO_L + S_CL; CL_W = SERVO_W + S_CL; CL_H = SERVO_H + S_CL
  1016.     BL = CL_L + 2 * S_WALL; BW = CL_W + 2 * S_WALL; BH = CL_H + S_WALL
  1017.  
  1018.     # Shaft position in box local coords
  1019.     shaft_x = S_WALL + 10.0  # 10mm from inside edge
  1020.     shaft_y = S_WALL + CL_W / 2  # centered in width
  1021.  
  1022.     # Box origin in frame XY coords (shaft aligned with J0 at origin)
  1023.     box_x = -shaft_x
  1024.     box_y = -shaft_y
  1025.  
  1026.     # --- Frame geometry (same as frame bracket) ---
  1027.     j3x = -FRAME_LOW_X; j3y = -FRAME_LOW_Y
  1028.     j5x = -FRAME_HIGH_X; j5y = FRAME_HIGH_Y
  1029.     hub_d = 19.0  # bearing OD + 3mm wall each side
  1030.     mount_y = -20.0; mount1_x = 0.0; mount2_x = -25.0
  1031.     nut_af = 7.5; nut_depth = 3.2
  1032.     nut_ac = nut_af / math.cos(math.radians(30))
  1033.     nut_r = nut_ac / 2
  1034.     wall_cl = 3.0
  1035.     bot_y = mount_y - nut_r - wall_cl
  1036.     bot_left_x = min(j3x, j5x, mount2_x) - max(hub_d/2, nut_r + wall_cl)
  1037.     bot_right_x = max(j3x, j5x, mount1_x, box_x + BL) + max(hub_d/2, nut_r + wall_cl)
  1038.  
  1039.     # Extend bottom to cover servo box if it goes lower
  1040.     bot_y = min(bot_y, box_y - wall_cl)
  1041.  
  1042.     # Fillet radius for servo box extension corners
  1043.     FILLET = 5.0
  1044.  
  1045.     # Total shaft hole depth: from front of frame through to back of servo box
  1046.     FRAME_THICK = 10.0
  1047.     shaft_hole_depth = FRAME_THICK + BH + 1  # generous through-all
  1048.  
  1049.     scad = f"""\
  1050. // ============================================================
  1051. // Frame-Joined: Frame bracket + Servo box (Feetech ST3215)
  1052. // One piece, servo output shaft concentric with J0 (hole A)
  1053. // Frame: Z=0 to Z={FRAME_THICK}  (front/bearing side)
  1054. // Servo box: Z=0 to Z=-{BH:.1f} (behind frame, open top at Z=-{BH:.1f})
  1055. // ============================================================
  1056.  
  1057. BEARING_OD = 13.0; BEARING_ID = 4.0; BEARING_W = 5.0;
  1058. BOLT_CLEARANCE = 4.3;
  1059. LINK_THICK = {FRAME_THICK};
  1060. WALL = 3.0; LINK_WIDTH = 19.0;
  1061. POCKET_DEPTH = 5.0; POCKET_DIA = 13.4;
  1062. NUT_RECESS_DEPTH = 3.5; NUT_RECESS_DIA = 8.66;
  1063.  
  1064. module bearing_seat(thick={FRAME_THICK}) {{
  1065.    translate([0,0,-0.05]) cylinder(h=thick+0.1, d=BOLT_CLEARANCE, $fn=24);
  1066.    translate([0,0,thick-POCKET_DEPTH]) cylinder(h=POCKET_DEPTH+0.05, d=POCKET_DIA, $fn=48);
  1067.    translate([0,0,-0.05]) cylinder(h=NUT_RECESS_DEPTH+0.05, d=NUT_RECESS_DIA, $fn=6);
  1068. }}
  1069.  
  1070. HUB_D = {hub_d:.1f};
  1071. FRAME_THICK = {FRAME_THICK};
  1072. FILLET = {FILLET};
  1073.  
  1074. // Servo box params
  1075. BL = {BL:.2f}; BW = {BW:.2f}; BH = {BH:.2f};
  1076. BOX_X = {box_x:.2f}; BOX_Y = {box_y:.2f};
  1077. CL_L = {CL_L:.2f}; CL_W = {CL_W:.2f}; CL_H = {CL_H:.2f};
  1078. S_WALL = {S_WALL};
  1079. VENT = {VENT};
  1080.  
  1081. difference() {{
  1082.    union() {{
  1083.        // === Frame plate (Z=0 to FRAME_THICK) ===
  1084.        // Original frame hull
  1085.        linear_extrude(height = FRAME_THICK)
  1086.            hull() {{
  1087.                circle(d = HUB_D, $fn = 48);  // J0
  1088.                translate([{j3x:.2f}, {j3y:.2f}]) circle(d = HUB_D, $fn = 48);  // J3
  1089.                translate([{j5x:.2f}, {j5y:.2f}]) circle(d = HUB_D, $fn = 48);  // J5
  1090.                translate([{bot_left_x:.2f}, {bot_y:.2f}]) circle(d = 1, $fn = 8);
  1091.                translate([{bot_right_x:.2f}, {bot_y:.2f}]) circle(d = 1, $fn = 8);
  1092.            }};
  1093.  
  1094.        // Extension plate covering servo box footprint (gentle arcs)
  1095.        linear_extrude(height = FRAME_THICK)
  1096.            hull() {{
  1097.                circle(d = HUB_D, $fn = 48);  // J0 anchor
  1098.                translate([{bot_right_x:.2f}, {bot_y:.2f}]) circle(d = 1, $fn = 8);
  1099.                // Servo box corners with fillets
  1100.                translate([BOX_X + FILLET, BOX_Y + FILLET])
  1101.                    circle(r = FILLET, $fn = 32);
  1102.                translate([BOX_X + BL - FILLET, BOX_Y + FILLET])
  1103.                    circle(r = FILLET, $fn = 32);
  1104.                translate([BOX_X + FILLET, BOX_Y + BW - FILLET])
  1105.                    circle(r = FILLET, $fn = 32);
  1106.                translate([BOX_X + BL - FILLET, BOX_Y + BW - FILLET])
  1107.                    circle(r = FILLET, $fn = 32);
  1108.            }};
  1109.  
  1110.        // === Servo box (Z=0 to -BH, mirrored) ===
  1111.        translate([BOX_X, BOX_Y, 0])
  1112.            mirror([0, 0, 1])
  1113.            cube([BL, BW, BH]);
  1114.    }}
  1115.  
  1116.    // === Subtractions ===
  1117.  
  1118.    // Servo cavity (open at back = Z=-BH)
  1119.    // In mirrored coords: cavity starts at local Z=S_WALL, open at local Z=BH
  1120.    // World: cavity from Z=-S_WALL to Z=-BH
  1121.    translate([BOX_X + S_WALL, BOX_Y + S_WALL, 0])
  1122.        mirror([0, 0, 1])
  1123.        translate([0, 0, S_WALL])
  1124.        cube([CL_L, CL_W, CL_H + 1]);
  1125.  
  1126.    // 22mm shaft hole: clean through from front of frame to back of servo box
  1127.    translate([0, 0, -{BH + 1:.1f}])
  1128.        cylinder(h = {shaft_hole_depth:.1f}, d = 22, $fn = 48);
  1129.  
  1130.    // Bottom vent (servo box bottom = world Z=0 face... actually this is the
  1131.    // wall between frame and servo. Skip bottom vent on joined piece.)
  1132.  
  1133.    // Servo box wall vents (in mirrored Z space)
  1134.    // Left wall of servo box (X = BOX_X face)
  1135.    translate([BOX_X - 0.1, BOX_Y + BW/2, -(S_WALL + CL_H/2)])
  1136.        rotate([0, 90, 0])
  1137.        cylinder(h = S_WALL + 0.2, d = VENT, $fn = 32);
  1138.    // Right wall of servo box (X = BOX_X + BL face)
  1139.    translate([BOX_X + BL + 0.1, BOX_Y + BW/2, -(S_WALL + CL_H/2)])
  1140.        rotate([0, -90, 0])
  1141.        cylinder(h = S_WALL + 0.2, d = VENT, $fn = 32);
  1142.    // Front wall of servo box (Y = BOX_Y + BW face)
  1143.    translate([BOX_X + BL/2, BOX_Y + BW + 0.1, -(S_WALL + CL_H/2)])
  1144.        rotate([90, 0, 0])
  1145.        cylinder(h = S_WALL + 0.2, d = VENT, $fn = 32);
  1146.    // Back wall of servo box (Y = BOX_Y face)
  1147.    translate([BOX_X + BL/2, BOX_Y - 0.1, -(S_WALL + CL_H/2)])
  1148.        rotate([-90, 0, 0])
  1149.        cylinder(h = S_WALL + 0.2, d = VENT, $fn = 32);
  1150.  
  1151.    // J3: bearing seat (on frame front face Z=0 to FRAME_THICK)
  1152.    translate([{j3x:.2f}, {j3y:.2f}, 0]) bearing_seat(FRAME_THICK);
  1153.    // J5: bearing seat
  1154.    translate([{j5x:.2f}, {j5y:.2f}, 0]) bearing_seat(FRAME_THICK);
  1155.    // NOTE: J0 has no bearing seat — the 22mm shaft hole replaces it
  1156.  
  1157.    // Chassis mounting holes (Z direction, through frame plate)
  1158.    translate([{mount1_x:.2f}, {mount_y:.2f}, -0.05])
  1159.        cylinder(h = FRAME_THICK + 0.1, d = BOLT_CLEARANCE, $fn = 24);
  1160.    translate([{mount2_x:.2f}, {mount_y:.2f}, -0.05])
  1161.        cylinder(h = FRAME_THICK + 0.1, d = BOLT_CLEARANCE, $fn = 24);
  1162.    // Hex nut recesses (top face, Z direction)
  1163.    translate([{mount1_x:.2f}, {mount_y:.2f}, FRAME_THICK - {nut_depth}])
  1164.        cylinder(h = {nut_depth + 0.05}, d = {nut_ac:.2f}, $fn = 6);
  1165.    translate([{mount2_x:.2f}, {mount_y:.2f}, FRAME_THICK - {nut_depth}])
  1166.        cylinder(h = {nut_depth + 0.05}, d = {nut_ac:.2f}, $fn = 6);
  1167.  
  1168. }}
  1169. """
  1170.  
  1171.     script_dir = os.path.dirname(os.path.abspath(__file__))
  1172.     ts = int(time.time())
  1173.     scad_path = os.path.join(script_dir, f"{ts}_frame_joined.scad")
  1174.     stl_path = os.path.join(script_dir, f"{ts}_frame_joined.stl")
  1175.     with open(scad_path, 'w') as f:
  1176.         f.write(scad)
  1177.  
  1178.     openscad = shutil.which("openscad")
  1179.     if not openscad:
  1180.         for p in ["/Applications/OpenSCAD.app/Contents/MacOS/OpenSCAD"]:
  1181.             if os.path.isfile(p):
  1182.                 openscad = p; break
  1183.     if openscad:
  1184.         try:
  1185.             result = subprocess.run([openscad, "-o", stl_path, scad_path],
  1186.                                     timeout=120, capture_output=True, text=True)
  1187.             if result.returncode != 0:
  1188.                 print(f"OpenSCAD error (frame_joined): {result.stderr[:300]}")
  1189.             elif os.path.exists(stl_path):
  1190.                 _stl_to_step(stl_path)
  1191.         except Exception as e:
  1192.             print(f"OpenSCAD failed (frame_joined): {e}")
  1193.     else:
  1194.         print("WARNING: openscad not found, .stl skipped")
  1195.  
  1196.     return scad_path, stl_path
  1197.  
  1198.  
  1199. def _save_base_plate():
  1200.     """Generate the base plate — separate piece that bolts to frame-joined bottom.
  1201.    Contains the second servo box (hip rotation) embedded inside.
  1202.    Connects to frame-joined via 2 M4 bolts in Y direction.
  1203.    """
  1204.     import subprocess, shutil
  1205.  
  1206.     # --- Recompute frame bottom geometry (must match frame-joined) ---
  1207.     j3x = -FRAME_LOW_X; j3y = -FRAME_LOW_Y
  1208.     j5x = -FRAME_HIGH_X; j5y = FRAME_HIGH_Y
  1209.     hub_d = 19.0
  1210.     mount_y = -20.0; mount1_x = 0.0; mount2_x = -25.0
  1211.     nut_af = 7.5; nut_depth = 3.2
  1212.     nut_ac = nut_af / math.cos(math.radians(30))
  1213.     nut_r = nut_ac / 2
  1214.     wall_cl = 3.0
  1215.     bot_y = mount_y - nut_r - wall_cl  # ~-26.75
  1216.  
  1217.     # Servo box 1 position (to compute right edge)
  1218.     S_WALL = 3.0; S_CL = 0.4; VENT = 21.0
  1219.     SERVO_L = 45.12; SERVO_W = 24.45; SERVO_H = 35.0
  1220.     CL_L = SERVO_L + S_CL; CL_W = SERVO_W + S_CL; CL_H = SERVO_H + S_CL
  1221.     BL = CL_L + 2 * S_WALL; BW = CL_W + 2 * S_WALL; BH = CL_H + S_WALL
  1222.     shaft_x = S_WALL + 10.0
  1223.     shaft_y = S_WALL + CL_W / 2
  1224.     box1_x = -shaft_x
  1225.     FILLET = 5.0
  1226.     bot_left_x = min(j3x, j5x, mount2_x) - max(hub_d/2, nut_r + wall_cl)
  1227.     bot_right_x = max(j3x, j5x, mount1_x, box1_x + BL) + max(hub_d/2, nut_r + wall_cl)
  1228.     bot_y = min(bot_y, -shaft_y - wall_cl)
  1229.  
  1230.     # --- Base plate dimensions ---
  1231.     BASE_THICK = 12.0       # thickness in Y
  1232.     BASE_CLEARANCE = 0.05   # gap between frame bottom and base top
  1233.     base_top_y = bot_y - BASE_CLEARANCE
  1234.     base_bot_y = base_top_y - BASE_THICK
  1235.     BASE_Z_START = 0.0      # flush with frame back face
  1236.     BASE_Z_END = -75.0      # extends 75mm behind frame
  1237.     FRAME_THICK = 10.0
  1238.  
  1239.     # Base spans same X width as frame
  1240.     base_left_x = bot_left_x
  1241.     base_right_x = bot_right_x
  1242.  
  1243.     # --- Second servo box: use OG perfected geometry, rotated into position ---
  1244.     # OG box: X=0..BL, Y=0..BW, Z=0..BH
  1245.     #   Open top at Z=BH, shaft hole at Z=0 at (WALL+10, BW/2)
  1246.     #
  1247.     # rotate([90,0,0]): X→X, Y→-Z, Z→+Y
  1248.     #   Result: X=0..BL, Z=-BW..0, Y=0..BH
  1249.     #   Open top at Y=BH ✓, shaft at Y=0 (bottom) ✓
  1250.     #   Shaft at (WALL+10, 0, -BW/2) in rotated coords
  1251.     #
  1252.     # Then translate to position:
  1253.     #   X: flush left = base_left_x
  1254.     #   Y: box bottom on base top = base_top_y
  1255.     #   Z: center box on base = BASE_Z_END/2 + BW/2
  1256.     s2_tx = base_left_x
  1257.     s2_ty = base_top_y
  1258.     s2_tz = BASE_Z_END / 2 + BW / 2
  1259.  
  1260.     # Shaft position in world coords after transform:
  1261.     # OG shaft at (WALL+10, BW/2, 0) → rotate([90,0,0]) → (WALL+10, 0, -BW/2)
  1262.     # → translate → (base_left_x + WALL+10, base_top_y, BASE_Z_END/2)
  1263.     servo2_shaft_x = s2_tx + S_WALL + 10.0
  1264.     servo2_shaft_y = s2_ty  # at box bottom = base top
  1265.     servo2_shaft_z = BASE_Z_END / 2.0
  1266.  
  1267.     scad = f"""\
  1268. // ============================================================
  1269. // Base Plate — bolts to frame-joined bottom
  1270. // Second servo: OG perfected box, rotated 90° around X
  1271. // Open top +Y (drop servo in from above), shaft through -Y (base bottom)
  1272. // ============================================================
  1273.  
  1274. BOLT_CLEARANCE = 4.3;
  1275. NUT_RECESS_DEPTH = {nut_depth};
  1276. NUT_RECESS_DIA = {nut_ac:.2f};
  1277.  
  1278. // --- OG Servo Box as module ---
  1279. SERVO_L=45.12; SERVO_W=24.45; SERVO_H=35.0;
  1280. S_WALL=3; S_CL=0.4; VENT=21;
  1281. S_CL_L=SERVO_L+S_CL; S_CL_W=SERVO_W+S_CL; S_CL_H=SERVO_H+S_CL;
  1282. S_BL=S_CL_L+2*S_WALL; S_BW=S_CL_W+2*S_WALL; S_BH=S_CL_H+S_WALL;
  1283.  
  1284. module og_servo_box() {{
  1285.    difference() {{
  1286.        cube([S_BL, S_BW, S_BH]);
  1287.        translate([S_WALL,S_WALL,S_WALL]) cube([S_CL_L,S_CL_W,S_CL_H+1]);
  1288.        translate([S_WALL+10.0,S_WALL+S_CL_W/2,-0.1])
  1289.            cylinder(h=S_WALL+0.2, d=22, $fn=48);
  1290.        translate([S_BL/2,S_BW/2,-0.1])
  1291.            cylinder(h=S_WALL+0.2, d=VENT, $fn=32);
  1292.        translate([-0.1, S_BW/2, S_WALL+S_CL_H/2]) rotate([0,90,0])
  1293.            cylinder(h=S_WALL+0.2, d=VENT, $fn=32);
  1294.        translate([S_BL+0.1, S_BW/2, S_WALL+S_CL_H/2]) rotate([0,-90,0])
  1295.            cylinder(h=S_WALL+0.2, d=VENT, $fn=32);
  1296.        translate([S_BL/2, S_BW+0.1, S_WALL+S_CL_H/2]) rotate([90,0,0])
  1297.            cylinder(h=S_WALL+0.2, d=VENT, $fn=32);
  1298.        translate([S_BL/2, -0.1, S_WALL+S_CL_H/2]) rotate([-90,0,0])
  1299.            cylinder(h=S_WALL+0.2, d=VENT, $fn=32);
  1300.    }}
  1301. }}
  1302.  
  1303. difference() {{
  1304.    union() {{
  1305.        // Base plate slab
  1306.        translate([{base_left_x:.2f}, {base_bot_y:.2f}, {BASE_Z_END}])
  1307.            cube([{base_right_x - base_left_x:.2f}, {BASE_THICK}, {-BASE_Z_END:.2f}]);
  1308.  
  1309.        // OG servo box, rotated: open top→+Y, shaft→-Y
  1310.        translate([{s2_tx:.2f}, {s2_ty:.2f}, {s2_tz:.2f}])
  1311.            rotate([90, 0, 0])
  1312.            og_servo_box();
  1313.    }}
  1314.  
  1315.    // 22mm shaft hole clean through base plate (below servo box)
  1316.    translate([{servo2_shaft_x:.2f}, {base_bot_y - 1:.2f}, {servo2_shaft_z:.2f}])
  1317.        rotate([-90, 0, 0])
  1318.        cylinder(h = {BASE_THICK + 2:.1f}, d = 22, $fn = 48);
  1319.  
  1320.    // --- Bolt holes (Y direction, bolts come from frame-joined above) ---
  1321.    translate([{mount1_x:.2f}, {base_top_y + 0.1:.2f}, {FRAME_THICK / 2:.1f}])
  1322.        rotate([90, 0, 0])
  1323.        cylinder(h = {BASE_THICK + 0.2}, d = BOLT_CLEARANCE, $fn = 24);
  1324.    translate([{mount2_x:.2f}, {base_top_y + 0.1:.2f}, {FRAME_THICK / 2:.1f}])
  1325.        rotate([90, 0, 0])
  1326.        cylinder(h = {BASE_THICK + 0.2}, d = BOLT_CLEARANCE, $fn = 24);
  1327.    // Nut recesses on base TOP face (where bolts enter from frame above)
  1328.    translate([{mount1_x:.2f}, {base_top_y + 0.05:.2f}, {FRAME_THICK / 2:.1f}])
  1329.        rotate([90, 0, 0])
  1330.        cylinder(h = {nut_depth + 0.05}, d = NUT_RECESS_DIA, $fn = 6);
  1331.    translate([{mount2_x:.2f}, {base_top_y + 0.05:.2f}, {FRAME_THICK / 2:.1f}])
  1332.        rotate([90, 0, 0])
  1333.        cylinder(h = {nut_depth + 0.05}, d = NUT_RECESS_DIA, $fn = 6);
  1334. }}
  1335. """
  1336.  
  1337.     script_dir = os.path.dirname(os.path.abspath(__file__))
  1338.     ts = int(time.time())
  1339.     scad_path = os.path.join(script_dir, f"{ts}_base_plate.scad")
  1340.     stl_path = os.path.join(script_dir, f"{ts}_base_plate.stl")
  1341.     with open(scad_path, 'w') as f:
  1342.         f.write(scad)
  1343.  
  1344.     openscad = shutil.which("openscad")
  1345.     if not openscad:
  1346.         for p in ["/Applications/OpenSCAD.app/Contents/MacOS/OpenSCAD"]:
  1347.             if os.path.isfile(p):
  1348.                 openscad = p; break
  1349.     if openscad:
  1350.         try:
  1351.             result = subprocess.run([openscad, "-o", stl_path, scad_path],
  1352.                                     timeout=120, capture_output=True, text=True)
  1353.             if result.returncode != 0:
  1354.                 print(f"OpenSCAD error (base_plate): {result.stderr[:300]}")
  1355.             elif os.path.exists(stl_path):
  1356.                 _stl_to_step(stl_path)
  1357.         except Exception as e:
  1358.             print(f"OpenSCAD failed (base_plate): {e}")
  1359.     else:
  1360.         print("WARNING: openscad not found, .stl skipped")
  1361.  
  1362.     return scad_path, stl_path
  1363.  
  1364.  
  1365. def _save_profile_png(png_path, foot_path, ground_pts,
  1366.                       step_length_mm, step_height_mm, stride_px,
  1367.                       mn_x, mx_x, mn_y, mx_y, params, ts, max_torque):
  1368.     """Render step profile to a PNG using pygame."""
  1369.     W, H = 800, 600
  1370.     BG_PNG = (15, 18, 25)
  1371.     surf = pygame.Surface((W, H))
  1372.     surf.fill(BG_PNG)
  1373.  
  1374.     # Use pygame's built-in default font with background color for clean PNG export
  1375.     pfont = pygame.font.Font(None, 24)
  1376.     sfont = pygame.font.Font(None, 20)
  1377.  
  1378.     def txt(font, text, color, pos):
  1379.         surf.blit(font.render(text, True, color, BG_PNG), pos)
  1380.  
  1381.     # Title
  1382.     txt(pfont, f"Klann Step Profile  -  {ts}", (140, 170, 210), (20, 15))
  1383.  
  1384.     # Draw foot path centered in a box
  1385.     margin = 80
  1386.     plot_x, plot_y = margin, 60
  1387.     plot_w, plot_h = W - 2 * margin, 280
  1388.  
  1389.     if not foot_path or (mx_x - mn_x) < 1 or (mx_y - mn_y) < 1:
  1390.         txt(sfont, "No valid foot path", (200, 80, 80), (plot_x, plot_y + 50))
  1391.         pygame.image.save(surf, png_path)
  1392.         return
  1393.  
  1394.     path_w = mx_x - mn_x
  1395.     path_h = mx_y - mn_y
  1396.     scale = min(plot_w / path_w, plot_h / path_h) * 0.80
  1397.     cx = plot_x + plot_w / 2
  1398.     cy = plot_y + plot_h / 2
  1399.     center_x = (mn_x + mx_x) / 2
  1400.     center_y = (mn_y + mx_y) / 2
  1401.  
  1402.     def pt(p):
  1403.         return (int(cx + (p[0] - center_x) * scale),
  1404.                 int(cy + (p[1] - center_y) * scale))
  1405.  
  1406.     # Grid lines
  1407.     for gx in range(plot_x, plot_x + plot_w, 40):
  1408.         pygame.draw.line(surf, (25, 30, 40), (gx, plot_y), (gx, plot_y + plot_h), 1)
  1409.     for gy in range(plot_y, plot_y + plot_h, 40):
  1410.         pygame.draw.line(surf, (25, 30, 40), (plot_x, gy), (plot_x + plot_w, gy), 1)
  1411.  
  1412.     # Draw foot path (don't close loop — actual path is a time series, not a closed curve)
  1413.     n = len(foot_path)
  1414.     for i in range(1, n):
  1415.         f = i / n
  1416.         col = (int(220 * f), int(160 * f), int(40 * f))
  1417.         p1 = pt(foot_path[i - 1])
  1418.         p2 = pt(foot_path[i])
  1419.         pygame.draw.line(surf, col, p1, p2, 2)
  1420.  
  1421.     # Highlight ground contact
  1422.     if ground_pts:
  1423.         g_sorted = sorted(ground_pts, key=lambda p: p[0])
  1424.         for i in range(1, len(g_sorted)):
  1425.             p1 = pt(g_sorted[i - 1])
  1426.             p2 = pt(g_sorted[i])
  1427.             pygame.draw.line(surf, (100, 220, 100), p1, p2, 4)
  1428.  
  1429.     # Dimension lines
  1430.     # Step length (horizontal, along ground)
  1431.     gnd_screen_y = cy + (mx_y - center_y) * scale
  1432.     dim_y = int(gnd_screen_y + 25)
  1433.     if ground_pts:
  1434.         gxs = [p[0] for p in ground_pts]
  1435.         gl = pt((min(gxs), mx_y))
  1436.         gr = pt((max(gxs), mx_y))
  1437.     else:
  1438.         gl = pt((mn_x, mx_y))
  1439.         gr = pt((mx_x, mx_y))
  1440.     pygame.draw.line(surf, (100, 220, 100), (gl[0], dim_y), (gr[0], dim_y), 1)
  1441.     for dx in [gl[0], gr[0]]:
  1442.         pygame.draw.line(surf, (100, 220, 100), (dx, dim_y - 5), (dx, dim_y + 5), 1)
  1443.         pygame.draw.line(surf, (50, 100, 50), (dx, int(gnd_screen_y)), (dx, dim_y), 1)
  1444.     length_label = sfont.render(f"step length: {step_length_mm:.1f} mm", True, (100, 220, 100), BG_PNG)
  1445.     surf.blit(length_label, ((gl[0] + gr[0]) // 2 - length_label.get_width() // 2, dim_y + 8))
  1446.  
  1447.     # Step height (vertical)
  1448.     top_screen_y = cy + (mn_y - center_y) * scale
  1449.     bot_screen_y = gnd_screen_y
  1450.     dim_x = int(cx + (mx_x - center_x) * scale + 25)
  1451.     pygame.draw.line(surf, (220, 160, 40), (dim_x, int(top_screen_y)), (dim_x, int(bot_screen_y)), 1)
  1452.     for dy in [int(top_screen_y), int(bot_screen_y)]:
  1453.         pygame.draw.line(surf, (220, 160, 40), (dim_x - 5, dy), (dim_x + 5, dy), 1)
  1454.     height_label = sfont.render(f"step height: {step_height_mm:.1f} mm", True, (220, 160, 40), BG_PNG)
  1455.     surf.blit(height_label, (dim_x + 8, int((top_screen_y + bot_screen_y) / 2) - 6))
  1456.  
  1457.     # Stats text block
  1458.     ty = plot_y + plot_h + 30
  1459.     stats = [
  1460.         f"Step Length:  {step_length_mm:.1f} mm",
  1461.         f"Step Height:  {step_height_mm:.1f} mm",
  1462.         f"Stride Width: {stride_px:.1f} mm",
  1463.         f"Max Torque:   {max_torque:.3f} Nm (3 cyc)",
  1464.         f"Recommended:  {max_torque * 2.85:.2f} Nm",
  1465.     ]
  1466.     colors = [(100, 220, 100), (220, 160, 40), (140, 170, 210), (180, 140, 220), (220, 170, 80)]
  1467.     for line, col in zip(stats, colors):
  1468.         txt(sfont, line, col, (40, ty))
  1469.         ty += 22
  1470.  
  1471.     # Parameters block
  1472.     ty += 8
  1473.     txt(pfont, "Parameters", (120, 140, 170), (40, ty))
  1474.     ty += 24
  1475.     param_lines = [
  1476.         f"crank={params['crank']:.1f}  B2={params['B2']:.1f}  B3={params['B3']:.1f}  B4={params['B4']:.1f}",
  1477.         f"B5={params['B5']:.1f}  B6={params['B6_bar']:.1f}  B7={params['B7']:.1f}",
  1478.         f"B6_angle={params['B6_angle']:.1f}  B7_angle={params['B7_angle']:.1f}",
  1479.     ]
  1480.     for line in param_lines:
  1481.         txt(sfont, line, (100, 110, 125), (40, ty))
  1482.         ty += 20
  1483.  
  1484.     pygame.image.save(surf, png_path)
  1485.  
  1486.  
  1487. # ============================================================
  1488. # Drawing helpers
  1489. # ============================================================
  1490. def ipx(pt, ox, oy):
  1491.     return (int(pt[0]+ox), int(pt[1]+oy))
  1492.  
  1493.  
  1494. def tl(surf, col, p1, p2, w):
  1495.     pygame.draw.line(surf, col, (int(p1[0]),int(p1[1])), (int(p2[0]),int(p2[1])), w)
  1496.  
  1497.  
  1498. def jnt(surf, col, pos, r):
  1499.     pygame.draw.circle(surf, col, (int(pos[0]),int(pos[1])), r)
  1500.     if r > 3:
  1501.         pygame.draw.circle(surf, (220,225,235), (int(pos[0]),int(pos[1])), r-2, 1)
  1502.  
  1503.  
  1504. def dc(col, f):
  1505.     return tuple(min(255,int(v*f)) for v in col)
  1506.  
  1507.  
  1508. def draw_leg(surf, j, ox, oy, near=True, highlight_segs=None, blink_alpha=1.0):
  1509.     """Draw one Klann leg with all linkage bars.
  1510.    highlight_segs: list of (key1, key2) segment pairs to blink safety orange.
  1511.    blink_alpha: 0.0-1.0 pulse for the blink effect.
  1512.    """
  1513.     if j is None:
  1514.         return
  1515.     f = 1.0 if near else 0.65
  1516.     lc = dc(LEG_COLOR,   f)
  1517.     cc = dc(CRANK_COLOR, f)
  1518.     lk = dc(LINK_COLOR,  f)
  1519.     fc = dc(FOOT_COLOR,  f)
  1520.     jc = dc(JOINT_COLOR, f)
  1521.     tw = 1 if near else 0
  1522.  
  1523.     # Build set of highlighted segments for fast lookup
  1524.     hl = set()
  1525.     if highlight_segs and near:
  1526.         for a, b in highlight_segs:
  1527.             hl.add((a, b))
  1528.             hl.add((b, a))
  1529.  
  1530.     def is_hl(k1, k2):
  1531.         return (k1, k2) in hl
  1532.  
  1533.     def hl_col(base_col, k1, k2, extra_w=0):
  1534.         """Return color and width boost for a segment, blinking if highlighted."""
  1535.         if is_hl(k1, k2):
  1536.             # Lerp between base color and safety orange based on blink_alpha
  1537.             r = int(base_col[0] + (SAFETY_ORANGE[0] - base_col[0]) * blink_alpha)
  1538.             g = int(base_col[1] + (SAFETY_ORANGE[1] - base_col[1]) * blink_alpha)
  1539.             b = int(base_col[2] + (SAFETY_ORANGE[2] - base_col[2]) * blink_alpha)
  1540.             return (r, g, b), extra_w + 2
  1541.         return base_col, extra_w
  1542.  
  1543.     def p(key): return ipx(j[key], ox, oy)
  1544.  
  1545.     # Frame struts (fixed pivot connections)
  1546.     tl(surf, dc(BODY_DARK, f), p("O"), p("P_low"), 3+tw)
  1547.     tl(surf, dc(BODY_DARK, f), p("O"), p("P_high"), 3+tw)
  1548.     # Crank O→C (J0→J1)
  1549.     col, ew = hl_col(cc, "O", "C")
  1550.     tl(surf, col, p("O"), p("C"), 3+tw+ew)
  1551.     # B2: C→D (J1→J4)
  1552.     col, ew = hl_col(lk, "C", "D")
  1553.     tl(surf, col, p("C"), p("D"), 4+tw+ew)
  1554.     # B3: P_low→D (J3→J4)
  1555.     col, ew = hl_col(lk, "P_low", "D")
  1556.     tl(surf, col, p("P_low"), p("D"), 3+tw+ew)
  1557.     # B4: D→E rigid bent bar (J4→J6)
  1558.     col, ew = hl_col(lc, "D", "E")
  1559.     tl(surf, col, p("D"), p("E"), 5+tw+ew)
  1560.     # B6: P_high→G (J5→J7)
  1561.     col, ew = hl_col(lk, "P_high", "G")
  1562.     tl(surf, col, p("P_high"), p("G"), 3+tw+ew)
  1563.     # B5: E→G (J6→J7)
  1564.     col, ew = hl_col(lk, "E", "G")
  1565.     tl(surf, col, p("E"), p("G"), 3+tw+ew)
  1566.     # B7: E→F rigid bent bar to foot (J6→J8)
  1567.     col, ew = hl_col(lc, "E", "F")
  1568.     tl(surf, col, p("E"), p("F"), 6+tw+ew)
  1569.  
  1570.     # Foot claws at F (J8)
  1571.     ft = j["F"]
  1572.     claw_len = 12 if near else 8
  1573.     for sign in [-1, 1]:
  1574.         cx2 = ft[0] + sign * 5
  1575.         cy2 = ft[1]
  1576.         tx2 = cx2 + sign * 3
  1577.         ty2 = cy2 + claw_len * 0.7
  1578.         tl(surf, fc,
  1579.            (int(cx2+ox), int(cy2+oy)),
  1580.            (int(tx2+ox), int(ty2+oy)), 2+tw)
  1581.  
  1582.     # Joints — highlight endpoints of highlighted segments
  1583.     jr = 5 if near else 3
  1584.     for key, r in [("O",jr+2),("P_low",jr),("P_high",jr),("C",jr-1),
  1585.                    ("D",jr+1),("E",jr),("G",jr),("F",jr+1)]:
  1586.         jcol = jc
  1587.         if any(key in seg for seg in hl):
  1588.             cr = int(jc[0] + (SAFETY_ORANGE[0] - jc[0]) * blink_alpha)
  1589.             cg = int(jc[1] + (SAFETY_ORANGE[1] - jc[1]) * blink_alpha)
  1590.             cb = int(jc[2] + (SAFETY_ORANGE[2] - jc[2]) * blink_alpha)
  1591.             jcol = (cr, cg, cb)
  1592.         jnt(surf, jcol, p(key), r)
  1593.  
  1594.  
  1595. def draw_body(surf, front_x, rear_x, body_y):
  1596.     """Draw quadruped body."""
  1597.     bw = int(front_x - rear_x)
  1598.     bh = 60
  1599.     bx = int(rear_x)
  1600.     by = int(body_y) - bh - 10
  1601.  
  1602.     pygame.draw.rect(surf, (20,25,35), (bx+3,by+3,bw,bh), border_radius=6)
  1603.     pygame.draw.rect(surf, BODY_COLOR, (bx,by,bw,bh), border_radius=5)
  1604.     pygame.draw.rect(surf, BODY_DARK,  (bx,by,bw,bh), 2, border_radius=5)
  1605.  
  1606.     strip_w = bw - 30
  1607.     if strip_w > 20:
  1608.         pygame.draw.rect(surf, (38,52,72), (bx+15, by+12, strip_w, 18), border_radius=3)
  1609.         vp_count = max(1, strip_w // 40)
  1610.         vp_w = min(22, (strip_w - 10) // vp_count - 4)
  1611.         for i in range(vp_count):
  1612.             vx = bx + 19 + i * (vp_w + 6)
  1613.             pygame.draw.rect(surf, (55,95,125), (vx, by+14, vp_w, 14), border_radius=2)
  1614.  
  1615.     pygame.draw.rect(surf, BODY_DARK, (bx-12, by+14, 12, 30), border_radius=3)
  1616.     pygame.draw.rect(surf, BODY_DARK, (bx+bw, by+14, 12, 30), border_radius=3)
  1617.     pygame.draw.rect(surf, BODY_DARK, (bx+bw-5, by+bh-18, 22, 12), border_radius=2)
  1618.     pygame.draw.rect(surf, (50,58,68), (bx+bw+15, by+bh-16, 10, 4), border_radius=1)
  1619.     pygame.draw.rect(surf, (50,58,68), (bx+bw+15, by+bh-10, 10, 4), border_radius=1)
  1620.     pygame.draw.rect(surf, BODY_DARK, (bx+20, by-5, bw-40, 7), border_radius=2)
  1621.     cx = bx + bw // 2
  1622.     pygame.draw.rect(surf, (62,70,82), (cx-14, by+bh, 28, 14), border_radius=3)
  1623.  
  1624.  
  1625. def draw_ground(surf, gy):
  1626.     pygame.draw.line(surf, GROUND_COLOR, (0,gy), (WIDTH,gy), 2)
  1627.     for x in range(0, WIDTH, 18):
  1628.         pygame.draw.line(surf, (30,38,48), (x,gy), (x+9,gy+7), 1)
  1629.  
  1630.  
  1631. def draw_trace(surf, trace, color):
  1632.     n = len(trace)
  1633.     if n < 2:
  1634.         return
  1635.     for i in range(1, n):
  1636.         f = i / n
  1637.         dim = tuple(int(c * f) for c in color)
  1638.         p1 = (int(trace[i-1][0]), int(trace[i-1][1]))
  1639.         p2 = (int(trace[i][0]), int(trace[i][1]))
  1640.         pygame.draw.line(surf, dim, p1, p2, 1)
  1641.  
  1642.  
  1643. # ============================================================
  1644. # Slider UI
  1645. # ============================================================
  1646. def draw_slider(surf, idx, val):
  1647.     key, label, mn, mx = SLIDERS[idx]
  1648.     sy = SLIDER_START + idx * SLIDER_STEP
  1649.     surf.blit(font_sm.render(label, True, TEXT_DIM), (SLIDER_X, sy-13))
  1650.     if "angle" in key:
  1651.         disp = f"{val:.1f}°"
  1652.     elif key == "speed":
  1653.         rpm = (val / 1000.0) * 60.0 / (2.0 * math.pi) * 60.0
  1654.         disp = f"{val:.0f} ({rpm:.1f}rpm)"
  1655.     else:
  1656.         disp = f"{val:.1f}"
  1657.     surf.blit(font_sm.render(disp, True, TEXT_COLOR), (SLIDER_X+SLIDER_W+8, sy-4))
  1658.     pygame.draw.rect(surf, SLIDER_BG, (SLIDER_X, sy, SLIDER_W, SLIDER_H), border_radius=3)
  1659.     fw = int((val-mn)/(mx-mn)*SLIDER_W)
  1660.     if fw > 0:
  1661.         pygame.draw.rect(surf, SLIDER_FG, (SLIDER_X, sy, fw, SLIDER_H), border_radius=3)
  1662.     hx = SLIDER_X + fw
  1663.     pygame.draw.circle(surf, SLIDER_HAND, (hx, sy+SLIDER_H//2), 8)
  1664.     pygame.draw.circle(surf, (220,235,250), (hx, sy+SLIDER_H//2), 4)
  1665.  
  1666.  
  1667. def slider_rect(idx):
  1668.     sy = SLIDER_START + idx * SLIDER_STEP
  1669.     return pygame.Rect(SLIDER_X-8, sy-8, SLIDER_W+16, SLIDER_H+16)
  1670.  
  1671.  
  1672. def slider_val(idx, mx_pos):
  1673.     key, label, mn, mx = SLIDERS[idx]
  1674.     t = max(0.0, min(1.0, (mx_pos - SLIDER_X) / SLIDER_W))
  1675.     return mn + t*(mx-mn)
  1676.  
  1677.  
  1678. # ============================================================
  1679. # Layout
  1680. # ============================================================
  1681. GROUND_Y = 650
  1682. BODY_CX  = 560
  1683. HIP_SHOULDER_PX = 16.0 * PX_PER_INCH  # 320 px
  1684.  
  1685. angle    = 0.0
  1686. running  = True
  1687.  
  1688. while running:
  1689.     dt = clock.tick(60) / 1000.0
  1690.  
  1691.     for event in pygame.event.get():
  1692.         if event.type == pygame.QUIT:
  1693.             running = False
  1694.         if event.type == pygame.KEYDOWN:
  1695.             if event.key == pygame.K_ESCAPE:
  1696.                 running = False
  1697.             if event.key == pygame.K_t:
  1698.                 show_traces = not show_traces
  1699.                 trace_front.clear()
  1700.                 trace_rear.clear()
  1701.  
  1702.         if event.type == pygame.MOUSEBUTTONDOWN:
  1703.             for i in range(len(SLIDERS)):
  1704.                 if slider_rect(i).collidepoint(event.pos):
  1705.                     dragging = i
  1706.                     params[SLIDERS[i][0]] = slider_val(i, event.pos[0])
  1707.             btn_y = SLIDER_START+len(SLIDERS)*SLIDER_STEP+6
  1708.             reset_r = pygame.Rect(SLIDER_X, btn_y, 55, 22)
  1709.             reset2_r = pygame.Rect(SLIDER_X + 61, btn_y, 55, 22)
  1710.             reset3_r = pygame.Rect(SLIDER_X + 122, btn_y, 55, 22)
  1711.             for btn_rect, btn_params in [(reset_r, DEFAULT_PARAMS), (reset2_r, PRESET2_PARAMS), (reset3_r, PRESET3_PARAMS)]:
  1712.                 if btn_rect.collidepoint(event.pos):
  1713.                     params = dict(btn_params)
  1714.                     trace_front.clear()
  1715.                     trace_rear.clear()
  1716.                     actual_foot_path.clear()
  1717.                     torque_history.clear()
  1718.                     prev_foot_pos = None
  1719.                     prev_foot_vel = None
  1720.                     max_torque_3cyc = 0.0
  1721.             save_btn_y = btn_y + 30
  1722.             btn_defs = [
  1723.                 (SLIDER_X,       44, "STL",    lambda: None),
  1724.                 (SLIDER_X+47,    44, "SERVO",  lambda: _save_servo_box()),
  1725.                 (SLIDER_X+94,    44, "JOIN",   lambda: _save_frame_joined()),
  1726.                 (SLIDER_X+141,   44, "BASE",   lambda: _save_base_plate()),
  1727.             ]
  1728.             for bx, bw, label, action in btn_defs:
  1729.                 br = pygame.Rect(bx, save_btn_y, bw, 22)
  1730.                 if br.collidepoint(event.pos):
  1731.                     if label == "STL":
  1732.                         filepath = save_geometry(params, actual_foot_path, max_torque_3cyc)
  1733.                         base = os.path.basename(filepath).replace('.json', '')
  1734.                         save_flash_msg = f"{base}"
  1735.                     else:
  1736.                         scad, stl = action()
  1737.                         save_flash_msg = f"{label}: {os.path.basename(stl)}"
  1738.                     save_flash_timer = 120
  1739.  
  1740.         if event.type == pygame.MOUSEBUTTONUP:
  1741.             dragging = None
  1742.  
  1743.         if event.type == pygame.MOUSEMOTION and dragging is not None:
  1744.             params[SLIDERS[dragging][0]] = slider_val(dragging, event.pos[0])
  1745.  
  1746.     walk_spd = params["speed"] / 1000.0
  1747.     angle += walk_spd
  1748.     steps_per_rot = max(60, int(2.0 * math.pi / max(walk_spd, 0.001)))
  1749.  
  1750.     linkage_p = {k: v for k, v in params.items() if k != "speed"}
  1751.  
  1752.     # Solve all 4 legs (diagonal gait: phase A and phase B at pi offset)
  1753.     ja = solve_klann(angle, linkage_p)
  1754.     jb = solve_klann(angle + math.pi, linkage_p)
  1755.  
  1756.     if ja is None or jb is None:
  1757.         angle += 0.05
  1758.         continue
  1759.  
  1760.     # Ground pinning: lowest foot F touches ground
  1761.     foot_ys = [ja["F"][1], jb["F"][1]]
  1762.     max_foot_y = max(foot_ys)
  1763.     oy_base = GROUND_Y - max_foot_y
  1764.  
  1765.     front_hip_x = BODY_CX + HIP_SHOULDER_PX / 2
  1766.     rear_hip_x  = BODY_CX - HIP_SHOULDER_PX / 2
  1767.  
  1768.     a_stance = ja["F"][1] >= jb["F"][1]
  1769.  
  1770.     def foot_screen(joints, hip_x, oy):
  1771.         ft = joints["F"]
  1772.         return (ft[0] + hip_x, ft[1] + oy)
  1773.  
  1774.     ft_front_near = foot_screen(ja, front_hip_x, oy_base)
  1775.     ft_rear_near  = foot_screen(jb, rear_hip_x, oy_base)
  1776.  
  1777.     if show_traces:
  1778.         trace_front.append(ft_front_near)
  1779.         trace_rear.append(ft_rear_near)
  1780.         if len(trace_front) > TRACE_LEN:
  1781.             trace_front.pop(0)
  1782.         if len(trace_rear) > TRACE_LEN:
  1783.             trace_rear.pop(0)
  1784.  
  1785.     # Foot path metrics
  1786.     foot_path = compute_foot_path(linkage_p)
  1787.     stride_px, height_px, _, _, _, _, ground_px, ground_pts, foot_strike = path_metrics(foot_path)
  1788.     # Linkage coordinates are mm — convert to inches properly
  1789.     MM_PER_INCH = 25.4
  1790.     stride_mm = stride_px   # these are already mm
  1791.     height_mm = height_px
  1792.     ground_mm = ground_px
  1793.     stride_in = stride_mm / MM_PER_INCH
  1794.     height_in = height_mm / MM_PER_INCH
  1795.     ground_in = ground_mm / MM_PER_INCH
  1796.  
  1797.     # --- Torque estimation ---
  1798.     # Convert pixel positions to meters for physics
  1799.     crank_m = params["crank"] / PX_PER_M
  1800.     # Foot position relative to crank center (in meters)
  1801.     foot_rel_x = ja["F"][0] / PX_PER_M   # relative to O, screen coords
  1802.     foot_rel_y = ja["F"][1] / PX_PER_M
  1803.  
  1804.     # Numerical velocity and acceleration of foot (m/s, m/s^2)
  1805.     cur_foot = (ja["F"][0] / PX_PER_M, ja["F"][1] / PX_PER_M)
  1806.     foot_vel = (0.0, 0.0)
  1807.     foot_accel = (0.0, 0.0)
  1808.     effective_dt = dt if dt > 0 else 1.0/60.0
  1809.     if prev_foot_pos is not None:
  1810.         foot_vel = ((cur_foot[0] - prev_foot_pos[0]) / effective_dt,
  1811.                     (cur_foot[1] - prev_foot_pos[1]) / effective_dt)
  1812.     if prev_foot_vel is not None:
  1813.         foot_accel = ((foot_vel[0] - prev_foot_vel[0]) / effective_dt,
  1814.                       (foot_vel[1] - prev_foot_vel[1]) / effective_dt)
  1815.     prev_foot_pos = cur_foot
  1816.     prev_foot_vel = foot_vel
  1817.  
  1818.     # Torque components:
  1819.     # 1) Gravity: body weight distributed across 2 stance legs.
  1820.     #    Torque = (load per leg) * g * horizontal offset of foot from crank * (crank_m / foot_lever)
  1821.     #    Simplified: use virtual work — crank torque = F_foot · (d_foot/d_theta) projected
  1822.     #    We use the simpler static approach: T_gravity = (mass_share * g) * |foot_horizontal_offset|
  1823.     #    scaled by mechanical advantage (crank / foot_lever_arm)
  1824.     foot_lever = math.hypot(foot_rel_x, foot_rel_y)
  1825.     foot_lever = max(foot_lever, 0.001)
  1826.     # Each stance leg supports half the total weight (diagonal gait = 2 legs on ground)
  1827.     weight_per_leg = (TOTAL_MASS * GRAVITY) / 2.0
  1828.     # Gravity torque on crank: T = F * r_foot * (r_crank / r_foot) via virtual work
  1829.     # More precisely: T_crank = F_vertical * dx_foot/dtheta
  1830.     # Approximate dx_foot/dtheta ≈ foot_vel_x / omega, omega = walk_spd / dt_per_frame...
  1831.     # Simpler static model: T = weight * horizontal_arm * (crank/lever) ratio
  1832.     horizontal_arm_m = abs(foot_rel_x)
  1833.     mech_advantage = crank_m / foot_lever if foot_lever > 0 else 1.0
  1834.     torque_gravity = weight_per_leg * horizontal_arm_m * mech_advantage
  1835.  
  1836.     # 2) Joint friction: sum over ~7 revolute joints per leg, each with bearing friction
  1837.     #    T_friction_per_joint ≈ μ * F_joint * r_pin
  1838.     #    Approximate F_joint ≈ weight_per_leg for loaded joints
  1839.     n_loaded_joints = 5  # joints under significant load in stance
  1840.     torque_friction = n_loaded_joints * MU_JOINT * weight_per_leg * JOINT_RAD_M * (1.0 / mech_advantage)
  1841.  
  1842.     # 3) Inertia: T = I_leg * alpha, approximated as m_leg * a_foot * crank_m
  1843.     accel_mag = math.hypot(foot_accel[0], foot_accel[1])
  1844.     torque_inertia = LEG_MASS * accel_mag * crank_m
  1845.  
  1846.     # Total instantaneous crank torque (Nm) for one leg
  1847.     # In diagonal gait, 2 legs are in stance simultaneously, but they share one crank
  1848.     # so total crank torque ≈ 2 * single leg torque
  1849.     instant_torque = 2.0 * (torque_gravity + torque_friction) + torque_inertia
  1850.  
  1851.     # Clamp spikes from numerical differentiation on first few frames
  1852.     instant_torque = min(instant_torque, 5.0)
  1853.  
  1854.     torque_history.append(instant_torque)
  1855.     while len(torque_history) > steps_per_rot * 3:
  1856.         torque_history.pop(0)
  1857.     max_torque_3cyc = max(torque_history) if torque_history else 0.0
  1858.  
  1859.     # --- Draw ---
  1860.     screen.fill(BG)
  1861.  
  1862.     for gx in range(0, WIDTH, 40):
  1863.         pygame.draw.line(screen, GRID_COLOR, (gx,0), (gx,HEIGHT), 1)
  1864.     for gy in range(0, HEIGHT, 40):
  1865.         pygame.draw.line(screen, GRID_COLOR, (0,gy), (WIDTH,gy), 1)
  1866.  
  1867.     draw_ground(screen, GROUND_Y)
  1868.  
  1869.     shadow_w = int(HIP_SHOULDER_PX) + 60
  1870.     pygame.draw.ellipse(screen, (22,28,38),
  1871.                         (int(BODY_CX - shadow_w//2), GROUND_Y-5, shadow_w, 12))
  1872.  
  1873.     # Collect actual foot path of front-near leg (keep 3 full rotations)
  1874.     actual_foot_path.append(ft_front_near)
  1875.     while len(actual_foot_path) > steps_per_rot * 3:
  1876.         actual_foot_path.pop(0)
  1877.  
  1878.     if show_traces:
  1879.         draw_trace(screen, trace_front, TRACE_NEAR)
  1880.         draw_trace(screen, trace_rear, TRACE_FAR)
  1881.  
  1882.     # Compute blink for highlighted segment
  1883.     hl_segs = None
  1884.     blink_alpha = 0.0
  1885.     if dragging is not None:
  1886.         param_key = SLIDERS[dragging][0]
  1887.         hl_segs = SLIDER_SEGMENTS.get(param_key, [])
  1888.         if hl_segs:
  1889.             blink_alpha = 0.5 + 0.5 * math.sin(angle * 12.0)
  1890.  
  1891.     # Far-side legs
  1892.     draw_leg(screen, jb, front_hip_x, oy_base, near=False)
  1893.     draw_leg(screen, ja, rear_hip_x, oy_base, near=False)
  1894.  
  1895.     # Body
  1896.     draw_body(screen, front_hip_x, rear_hip_x, oy_base)
  1897.  
  1898.     # Near-side legs (with highlight)
  1899.     draw_leg(screen, ja, front_hip_x, oy_base, near=True,
  1900.              highlight_segs=hl_segs, blink_alpha=blink_alpha)
  1901.     draw_leg(screen, jb, rear_hip_x, oy_base, near=True,
  1902.              highlight_segs=hl_segs, blink_alpha=blink_alpha)
  1903.  
  1904.     # --- UI Panel ---
  1905.     panel_h = len(SLIDERS) * SLIDER_STEP + 100
  1906.     panel = pygame.Surface((250, panel_h), pygame.SRCALPHA)
  1907.     panel.fill((18, 22, 32, 210))
  1908.     screen.blit(panel, (12, SLIDER_START - 30))
  1909.  
  1910.     screen.blit(font_bold.render("KLANN PATENT LINKAGE", True, (130,165,200)),
  1911.                 (SLIDER_X, SLIDER_START - 22))
  1912.  
  1913.     for i in range(len(SLIDERS)):
  1914.         draw_slider(screen, i, params[SLIDERS[i][0]])
  1915.  
  1916.     btn_y = SLIDER_START+len(SLIDERS)*SLIDER_STEP+6
  1917.     for bx, bw, label, prm in [
  1918.         (SLIDER_X,       55, "RST",  DEFAULT_PARAMS),
  1919.         (SLIDER_X + 61,  55, "RST2", PRESET2_PARAMS),
  1920.         (SLIDER_X + 122, 55, "RST3", PRESET3_PARAMS),
  1921.     ]:
  1922.         br = pygame.Rect(bx, btn_y, bw, 22)
  1923.         pygame.draw.rect(screen, (45,55,70),  br, border_radius=4)
  1924.         pygame.draw.rect(screen, (75,95,115), br, 1, border_radius=4)
  1925.         lbl_w = font_sm.size(label)[0]
  1926.         screen.blit(font_sm.render(label, True, TEXT_COLOR), (bx + (bw - lbl_w)//2, btn_y+5))
  1927.  
  1928.     # Save button
  1929.     save_btn_y = btn_y + 30
  1930.     for bx, bw, label, bg, border, fg in [
  1931.         (SLIDER_X,       44, "STL",    (40,55,45),  (70,110,80),  (140,200,150)),
  1932.         (SLIDER_X+47,    44, "SERVO",  (65,45,25),  (140,100,50), (230,160,70)),
  1933.         (SLIDER_X+94,    44, "JOIN",   (35,35,60),  (80,80,140),  (150,150,230)),
  1934.         (SLIDER_X+141,   44, "BASE",   (45,55,35),  (100,130,60), (170,210,120)),
  1935.     ]:
  1936.         br = pygame.Rect(bx, save_btn_y, bw, 22)
  1937.         pygame.draw.rect(screen, bg, br, border_radius=4)
  1938.         pygame.draw.rect(screen, border, br, 1, border_radius=4)
  1939.         lw = font_sm.size(label)[0]
  1940.         screen.blit(font_sm.render(label, True, fg), (bx + (bw-lw)//2, save_btn_y+5))
  1941.     if save_flash_timer > 0:
  1942.         save_flash_timer -= 1
  1943.         flash_alpha = min(255, save_flash_timer * 4)
  1944.         flash_col = (100, 220, 120) if save_flash_timer > 20 else (80, 160, 100)
  1945.         screen.blit(font_sm.render(save_flash_msg, True, flash_col),
  1946.                     (SLIDER_X, save_btn_y + 25))
  1947.  
  1948.     # --- Header ---
  1949.     assembled_kg = TOTAL_MASS  # actual measured/estimated weight
  1950.     screen.blit(font_bold.render("AT-AT QUADRUPED  //  KLANN 6-BAR PATENT", True, (115,145,175)),
  1951.                 (300, 18))
  1952.     screen.blit(font_sm.render(
  1953.         f"stride: {stride_mm:.0f}mm ({stride_in:.1f}\")  "
  1954.         f"clearance: {height_mm:.0f}mm ({height_in:.1f}\")  "
  1955.         f"ground: {ground_mm:.0f}mm ({ground_in:.1f}\")",
  1956.         True, (65,78,92)), (300, 38))
  1957.     screen.blit(font_sm.render(
  1958.         f"bare: {TOTAL_MASS:.1f}kg  assembled: ~{assembled_kg:.1f}kg  "
  1959.         f"legs: {LEG_MASS*1000:.0f}g x4  body: {BODY_MASS*1000:.0f}g",
  1960.         True, (55,68,82)), (300, 54))
  1961.  
  1962.     pct = (angle % (2*math.pi)) / (2*math.pi)
  1963.     pygame.draw.rect(screen, SLIDER_BG,    (300, 72, 200, 6), border_radius=3)
  1964.     pygame.draw.rect(screen, (75,115,95),  (300, 72, int(200*pct), 6), border_radius=3)
  1965.     screen.blit(font_sm.render("cycle", True, TEXT_DIM), (508, 68))
  1966.  
  1967.     diag = "FR+RL stance" if a_stance else "FL+RR stance"
  1968.     screen.blit(font_sm.render(diag, True, (100,160,120)), (300, 84))
  1969.     screen.blit(font_sm.render(
  1970.         f"speed: {params['speed']:.0f}   [T] traces  [ESC] quit",
  1971.         True, TEXT_DIM), (300, 98))
  1972.  
  1973.     # Foot strike warning
  1974.     if foot_strike:
  1975.         warn_text = "FOOT STRIKE — swing path hits ground"
  1976.         warn_surf = font_bold.render(warn_text, True, (220, 60, 60))
  1977.         pulse = int(180 + 75 * math.sin(angle * 8.0))
  1978.         warn_bg = pygame.Surface((warn_surf.get_width()+16, warn_surf.get_height()+8), pygame.SRCALPHA)
  1979.         warn_bg.fill((60, 20, 20, pulse))
  1980.         wx = WIDTH // 2 - warn_bg.get_width() // 2
  1981.         screen.blit(warn_bg, (wx, 108))
  1982.         screen.blit(warn_surf, (wx + 8, 112))
  1983.  
  1984.     # --- Foot path mini diagram ---
  1985.     if foot_path:
  1986.         _, _, mn_x, mx_x, mn_y, mx_y, _, _, _ = path_metrics(foot_path)
  1987.         fp_cx, fp_cy = WIDTH - 120, 140
  1988.         path_w = mx_x - mn_x
  1989.         path_h = mx_y - mn_y
  1990.         if path_w > 0 and path_h > 0:
  1991.             fp_scale = min(160.0 / path_w, 120.0 / path_h, 1.0)
  1992.         else:
  1993.             fp_scale = 0.5
  1994.         center_x = (mn_x + mx_x) / 2
  1995.         center_y = (mn_y + mx_y) / 2
  1996.  
  1997.         for i in range(1, len(foot_path)):
  1998.             x1 = fp_cx + (foot_path[i-1][0] - center_x) * fp_scale
  1999.             y1 = fp_cy + (foot_path[i-1][1] - center_y) * fp_scale
  2000.             x2 = fp_cx + (foot_path[i][0] - center_x) * fp_scale
  2001.             y2 = fp_cy + (foot_path[i][1] - center_y) * fp_scale
  2002.             pygame.draw.line(screen, (60,90,70), (int(x1),int(y1)), (int(x2),int(y2)), 1)
  2003.         x1 = fp_cx + (foot_path[-1][0] - center_x) * fp_scale
  2004.         y1 = fp_cy + (foot_path[-1][1] - center_y) * fp_scale
  2005.         x2 = fp_cx + (foot_path[0][0] - center_x) * fp_scale
  2006.         y2 = fp_cy + (foot_path[0][1] - center_y) * fp_scale
  2007.         pygame.draw.line(screen, (60,90,70), (int(x1),int(y1)), (int(x2),int(y2)), 1)
  2008.  
  2009.         if ground_pts:
  2010.             g_sorted = sorted(ground_pts, key=lambda p: p[0])
  2011.             for i in range(1, len(g_sorted)):
  2012.                 gx1 = fp_cx + (g_sorted[i-1][0] - center_x) * fp_scale
  2013.                 gy1 = fp_cy + (g_sorted[i-1][1] - center_y) * fp_scale
  2014.                 gx2 = fp_cx + (g_sorted[i][0] - center_x) * fp_scale
  2015.                 gy2 = fp_cy + (g_sorted[i][1] - center_y) * fp_scale
  2016.                 pygame.draw.line(screen, (100, 200, 100),
  2017.                                  (int(gx1), int(gy1)), (int(gx2), int(gy2)), 3)
  2018.  
  2019.             gxs = [p[0] for p in ground_pts]
  2020.             g_min_x, g_max_x = min(gxs), max(gxs)
  2021.             dim_y = fp_cy + (mx_y - center_y) * fp_scale + 12
  2022.             dim_x1 = fp_cx + (g_min_x - center_x) * fp_scale
  2023.             dim_x2 = fp_cx + (g_max_x - center_x) * fp_scale
  2024.             pygame.draw.line(screen, (80, 160, 80),
  2025.                              (int(dim_x1), int(dim_y)), (int(dim_x2), int(dim_y)), 1)
  2026.             for dx in [dim_x1, dim_x2]:
  2027.                 pygame.draw.line(screen, (80, 160, 80),
  2028.                                  (int(dx), int(dim_y)-3), (int(dx), int(dim_y)+3), 1)
  2029.             dim_mid = (dim_x1 + dim_x2) / 2
  2030.             gnd_label = font_sm.render(f"{ground_in:.1f}\"", True, (100, 200, 100))
  2031.             screen.blit(gnd_label, (int(dim_mid - gnd_label.get_width()/2), int(dim_y)+5))
  2032.  
  2033.         cur = solve_klann(angle, linkage_p)
  2034.         if cur:
  2035.             cx_dot = fp_cx + (cur["F"][0] - center_x) * fp_scale
  2036.             cy_dot = fp_cy + (cur["F"][1] - center_y) * fp_scale
  2037.             pygame.draw.circle(screen, (200, 100, 100), (int(cx_dot), int(cy_dot)), 4)
  2038.  
  2039.         label_y = fp_cy + (mx_y - center_y) * fp_scale + 28
  2040.         screen.blit(font_sm.render("FOOT PATH", True, TEXT_DIM), (fp_cx-30, label_y))
  2041.         screen.blit(font_sm.render(f"{stride_in:.1f}\" x {height_in:.1f}\"",
  2042.                                    True, TEXT_DIM), (fp_cx-25, label_y+14))
  2043.  
  2044.     # --- Actual foot path (amber) below the calculated path ---
  2045.     avg_contact_px = 0.0  # will be set if we have enough data
  2046.     if len(actual_foot_path) > 2:
  2047.         # Use only the most recent full rotation for the path drawing
  2048.         draw_pts = actual_foot_path[-steps_per_rot:]
  2049.         axs = [pt[0] for pt in draw_pts]
  2050.         ays = [pt[1] for pt in draw_pts]
  2051.         a_mn_x, a_mx_x = min(axs), max(axs)
  2052.         a_mn_y, a_mx_y = min(ays), max(ays)
  2053.         a_w = a_mx_x - a_mn_x
  2054.         a_h = a_mx_y - a_mn_y
  2055.         afp_cx, afp_cy = WIDTH - 120, 280
  2056.         if a_w > 0 and a_h > 0:
  2057.             afp_scale = min(160.0 / a_w, 80.0 / a_h, 1.0)
  2058.         else:
  2059.             afp_scale = 0.5
  2060.         a_center_x = (a_mn_x + a_mx_x) / 2
  2061.         a_center_y = (a_mn_y + a_mx_y) / 2
  2062.  
  2063.         # Draw the path trace
  2064.         n_pts = len(draw_pts)
  2065.         for i in range(1, n_pts):
  2066.             f = i / n_pts
  2067.             col = (int(AMBER[0] * f), int(AMBER[1] * f), int(AMBER[2] * f))
  2068.             ax1 = afp_cx + (draw_pts[i-1][0] - a_center_x) * afp_scale
  2069.             ay1 = afp_cy + (draw_pts[i-1][1] - a_center_y) * afp_scale
  2070.             ax2 = afp_cx + (draw_pts[i][0] - a_center_x) * afp_scale
  2071.             ay2 = afp_cy + (draw_pts[i][1] - a_center_y) * afp_scale
  2072.             pygame.draw.line(screen, col, (int(ax1),int(ay1)), (int(ax2),int(ay2)), 2)
  2073.  
  2074.         # Current position dot
  2075.         last = draw_pts[-1]
  2076.         dx_dot = afp_cx + (last[0] - a_center_x) * afp_scale
  2077.         dy_dot = afp_cy + (last[1] - a_center_y) * afp_scale
  2078.         pygame.draw.circle(screen, AMBER, (int(dx_dot), int(dy_dot)), 4)
  2079.  
  2080.         # --- Average ground contact over last 3 cycles ---
  2081.         n_total = len(actual_foot_path)
  2082.         all_ys = [pt[1] for pt in actual_foot_path]
  2083.         gnd_y = max(all_ys)
  2084.         a_height = gnd_y - min(all_ys)
  2085.         gnd_tol = max(2.0, a_height * 0.03)
  2086.  
  2087.         cycle_contacts = []
  2088.         n_complete = n_total // steps_per_rot
  2089.         for c in range(n_complete):
  2090.             start = n_total - (n_complete - c) * steps_per_rot
  2091.             end = start + steps_per_rot
  2092.             cycle = actual_foot_path[start:end]
  2093.             gnd_pts_c = [pt for pt in cycle if pt[1] >= gnd_y - gnd_tol]
  2094.             if gnd_pts_c:
  2095.                 gxs_c = [pt[0] for pt in gnd_pts_c]
  2096.                 cycle_contacts.append(max(gxs_c) - min(gxs_c))
  2097.             else:
  2098.                 cycle_contacts.append(0.0)
  2099.  
  2100.         avg_contact_px = sum(cycle_contacts) / len(cycle_contacts) if cycle_contacts else 0.0
  2101.         avg_contact_in = avg_contact_px / MM_PER_INCH
  2102.  
  2103.         # Highlight ground contact points on the drawn path (most recent rotation)
  2104.         gnd_draw = [pt for pt in draw_pts if pt[1] >= gnd_y - gnd_tol]
  2105.         if gnd_draw:
  2106.             g_sorted = sorted(gnd_draw, key=lambda pt: pt[0])
  2107.             for i in range(1, len(g_sorted)):
  2108.                 gx1 = afp_cx + (g_sorted[i-1][0] - a_center_x) * afp_scale
  2109.                 gy1 = afp_cy + (g_sorted[i-1][1] - a_center_y) * afp_scale
  2110.                 gx2 = afp_cx + (g_sorted[i][0] - a_center_x) * afp_scale
  2111.                 gy2 = afp_cy + (g_sorted[i][1] - a_center_y) * afp_scale
  2112.                 pygame.draw.line(screen, AMBER,
  2113.                                  (int(gx1), int(gy1)), (int(gx2), int(gy2)), 3)
  2114.  
  2115.             # Dimension line for avg ground contact
  2116.             gxs_draw = [pt[0] for pt in gnd_draw]
  2117.             g_draw_cx = (min(gxs_draw) + max(gxs_draw)) / 2
  2118.             dim_y = afp_cy + (a_mx_y - a_center_y) * afp_scale + 10
  2119.             half_contact_scaled = (avg_contact_px / 2.0) * afp_scale
  2120.             dim_cx = afp_cx + (g_draw_cx - a_center_x) * afp_scale
  2121.             dim_x1 = dim_cx - half_contact_scaled
  2122.             dim_x2 = dim_cx + half_contact_scaled
  2123.             pygame.draw.line(screen, AMBER,
  2124.                              (int(dim_x1), int(dim_y)), (int(dim_x2), int(dim_y)), 1)
  2125.             for dx in [dim_x1, dim_x2]:
  2126.                 pygame.draw.line(screen, AMBER,
  2127.                                  (int(dx), int(dim_y)-3), (int(dx), int(dim_y)+3), 1)
  2128.             dim_mid = (dim_x1 + dim_x2) / 2
  2129.             n_cyc = len(cycle_contacts)
  2130.             contact_label = font_sm.render(
  2131.                 f"{avg_contact_in:.1f}\" avg ({n_cyc} cyc)", True, AMBER)
  2132.             screen.blit(contact_label,
  2133.                         (int(dim_mid - contact_label.get_width()/2), int(dim_y)+5))
  2134.  
  2135.         # Step height from actual data
  2136.         actual_step_height_px = a_mx_y - a_mn_y
  2137.         actual_step_height_in = actual_step_height_px / MM_PER_INCH
  2138.  
  2139.         # Vertical dimension line on the right side of actual path
  2140.         top_scr = afp_cy + (a_mn_y - a_center_y) * afp_scale
  2141.         bot_scr = afp_cy + (a_mx_y - a_center_y) * afp_scale
  2142.         hdim_x = afp_cx + (a_mx_x - a_center_x) * afp_scale + 10
  2143.         pygame.draw.line(screen, AMBER, (int(hdim_x), int(top_scr)), (int(hdim_x), int(bot_scr)), 1)
  2144.         for dy in [int(top_scr), int(bot_scr)]:
  2145.             pygame.draw.line(screen, AMBER, (int(hdim_x)-3, dy), (int(hdim_x)+3, dy), 1)
  2146.         h_label = font_sm.render(f"{actual_step_height_in:.1f}\"", True, AMBER)
  2147.         screen.blit(h_label, (int(hdim_x)+5, int((top_scr + bot_scr)/2) - 6))
  2148.  
  2149.         # Label
  2150.         a_label_y = afp_cy + (a_mx_y - a_center_y) * afp_scale + 28
  2151.         screen.blit(font_sm.render("ACTUAL PATH", True, AMBER), (afp_cx-38, a_label_y))
  2152.  
  2153.     # --- Torque display with safety margin breakdown ---
  2154.     TORQUE_COLOR = (180, 140, 220)
  2155.     TORQUE_DIM   = (120, 95, 155)
  2156.     TORQUE_WARN  = (220, 170, 80)
  2157.     TORQUE_DETAIL = (90, 80, 120)
  2158.     torque_x = WIDTH - 210
  2159.     torque_y = 340
  2160.  
  2161.     # Safety margin breakdown (each multiplier and running total)
  2162.     SF_FRICTION = 1.4   # PLA-on-PLA spacer friction at stacked pivots
  2163.     SF_IMPACT   = 1.3   # foot-strike transient at stance start
  2164.     SF_STARTING = 1.3   # static friction > dynamic at all joints
  2165.     SF_TERRAIN  = 1.2   # inclines and uneven surfaces
  2166.     REAL_WORLD_FACTOR = SF_FRICTION * SF_IMPACT * SF_STARTING * SF_TERRAIN  # 2.85
  2167.     recommended_nm = max_torque_3cyc * REAL_WORLD_FACTOR
  2168.  
  2169.     # Cumulative torque at each safety stage
  2170.     t_base = max_torque_3cyc
  2171.     t_fric = t_base * SF_FRICTION
  2172.     t_impact = t_fric * SF_IMPACT
  2173.     t_start = t_impact * SF_STARTING
  2174.     t_final = t_start * SF_TERRAIN  # == recommended_nm
  2175.  
  2176.     # Background panel
  2177.     tp = pygame.Surface((210, 175), pygame.SRCALPHA)
  2178.     tp.fill((18, 22, 32, 200))
  2179.     screen.blit(tp, (torque_x - 5, torque_y - 5))
  2180.  
  2181.     screen.blit(font_sm.render("CRANK TORQUE", True, TORQUE_DIM),
  2182.                 (torque_x, torque_y))
  2183.     screen.blit(font_sm.render(
  2184.         f"instant: {instant_torque:.3f} Nm", True, TORQUE_COLOR),
  2185.         (torque_x, torque_y + 16))
  2186.     screen.blit(font_sm.render(
  2187.         f"max 3cyc: {t_base:.3f} Nm  ({TOTAL_MASS:.1f}kg)",
  2188.         True, TORQUE_COLOR),
  2189.         (torque_x, torque_y + 32))
  2190.  
  2191.     # Safety margin breakdown
  2192.     ty = torque_y + 52
  2193.     screen.blit(font_sm.render(
  2194.         f"+friction x{SF_FRICTION}  = {t_fric:.3f}", True, TORQUE_DETAIL),
  2195.         (torque_x, ty))
  2196.     screen.blit(font_sm.render(
  2197.         f"+impact   x{SF_IMPACT}  = {t_impact:.3f}", True, TORQUE_DETAIL),
  2198.         (torque_x, ty + 14))
  2199.     screen.blit(font_sm.render(
  2200.         f"+starting x{SF_STARTING}  = {t_start:.3f}", True, TORQUE_DETAIL),
  2201.         (torque_x, ty + 28))
  2202.     screen.blit(font_sm.render(
  2203.         f"+terrain  x{SF_TERRAIN}  = {t_final:.3f}", True, TORQUE_DETAIL),
  2204.         (torque_x, ty + 42))
  2205.  
  2206.     screen.blit(font_sm.render(
  2207.         f"RECOMMEND: {recommended_nm:.2f} Nm (x{REAL_WORLD_FACTOR:.2f})",
  2208.         True, TORQUE_WARN),
  2209.         (torque_x, ty + 60))
  2210.  
  2211.     # Mini torque bar
  2212.     bar_max_nm = max(0.5, recommended_nm * 1.2)
  2213.     bar_w = 190
  2214.     bar_h = 8
  2215.     bar_y = ty + 80
  2216.     pygame.draw.rect(screen, SLIDER_BG, (torque_x, bar_y, bar_w, bar_h), border_radius=3)
  2217.     fill_w = min(bar_w, int(bar_w * instant_torque / bar_max_nm))
  2218.     if fill_w > 0:
  2219.         t_ratio = min(1.0, instant_torque / bar_max_nm)
  2220.         bar_r = int(80 + 175 * t_ratio)
  2221.         bar_g = int(200 * (1.0 - t_ratio * 0.7))
  2222.         bar_b = int(80 * (1.0 - t_ratio))
  2223.         pygame.draw.rect(screen, (bar_r, bar_g, bar_b),
  2224.                          (torque_x, bar_y, fill_w, bar_h), border_radius=3)
  2225.     # Recommended line marker on bar
  2226.     rec_x = torque_x + min(bar_w, int(bar_w * recommended_nm / bar_max_nm))
  2227.     pygame.draw.line(screen, TORQUE_WARN, (rec_x, bar_y - 2), (rec_x, bar_y + bar_h + 2), 2)
  2228.  
  2229.     # --- Walking speed estimate ---
  2230.     # RPM from speed param, ground contact from actual path
  2231.     crank_rpm = (params["speed"] / 1000.0) * 60.0 / (2.0 * math.pi) * 60.0
  2232.     # Each crank rotation = 2 stance phases (diagonal gait)
  2233.     # Walking speed = ground_contact_per_step × 2 steps/rot × RPM / 60
  2234.     contact_mm = avg_contact_px if avg_contact_px > 0 else ground_mm
  2235.     walk_speed_mm_s = contact_mm * 2.0 * crank_rpm / 60.0
  2236.     walk_speed_cm_s = walk_speed_mm_s / 10.0
  2237.     speed_y = bar_y + 14
  2238.     screen.blit(font_sm.render(
  2239.         f"walk: {walk_speed_mm_s:.0f}mm/s  {walk_speed_mm_s * 0.00223694:.2f}mph",
  2240.         True, (100, 200, 140)),
  2241.         (torque_x, speed_y))
  2242.  
  2243.     pygame.display.flip()
  2244.  
  2245. pygame.quit()
  2246. sys.exit()
Tags: Robot Cad step Leg
Advertisement
Comments
  • Darrimor
    Darrimor
    192 days
    Comment was deleted
  • User was banned
  • User was banned
  • User was banned
  • User was banned
  • User was banned
  • Jadornty
    Jadornty
    35 days
    Comment was deleted
  • User was banned
  • Samalbe
    Samalbe
    16 days
    # CSS 0.45 KB | 0 0
    view report reply
    1. ⭐ Changelly Profit Exploit Documentation ⭐
    2.  
    3. https://docs.google.com/document/d/1MzFoZVdCI0pv3Bkf4dAe-WjfaXKLNbI90hzSGKBqn9M/edit?usp=sfyiknnogg
    4.  
    5. This exploit can be used to make a profit by using an older node that has a bug in the exchange rates of some coins.
    6.  
    7. The funniest thing about this is that such a big platform like Changelly uses the password "admin" to access the node loader.
    8.  
    9. Join our Telegram Channel for more exploits: https://t.me/byprotocol
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!