API tools faq
paste
Hadlock

walker.py

Hadlock
Mar 14th, 2026
121
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 61.40 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.100    # kg per leg
  44. BODY_MASS  = 0.500    # kg body
  45. TOTAL_MASS = BODY_MASS + 4 * LEG_MASS  # 0.9 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(DEFAULT_PARAMS)
  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 * 5.7),
  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. _SCAD_PREAMBLE = """\
  495. // 624ZZ bearing dimensions
  496. BEARING_OD     = 13.0;
  497. BEARING_ID     =  4.0;
  498. BEARING_W      =  5.0;
  499. BOLT_CLEARANCE =  4.2;
  500.  
  501. // Link geometry
  502. LINK_THICK     =  6.0;
  503. WALL           =  3.0;
  504. LINK_WIDTH     = BEARING_OD + 2 * WALL;  // 19mm
  505. POCKET_DEPTH   =  5.0;
  506. POCKET_DIA     = 13.1;
  507.  
  508. module bearing_seat(thick=LINK_THICK) {
  509.    translate([0, 0, -0.05])
  510.        cylinder(h = thick + 0.1, d = BOLT_CLEARANCE, $fn = 24);
  511.    translate([0, 0, thick - POCKET_DEPTH])
  512.        cylinder(h = POCKET_DEPTH + 0.05, d = POCKET_DIA, $fn = 48);
  513. }
  514.  
  515. module straight_bar(c2c) {
  516.    difference() {
  517.        hull() {
  518.            cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  519.            translate([c2c, 0, 0])
  520.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  521.        }
  522.        bearing_seat();
  523.        translate([c2c, 0, 0]) bearing_seat();
  524.    }
  525. }
  526.  
  527. // Bent bar: two straight arms joined at vertex (boomerang/L shape)
  528. // h1x,h1y = first endpoint, h2x,h2y = second endpoint, vertex at (0,0)
  529. module bent_bar(h1x, h1y, h2x, h2y) {
  530.    difference() {
  531.        union() {
  532.            // Arm 1: vertex to endpoint 1
  533.            hull() {
  534.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  535.                translate([h1x, h1y, 0])
  536.                    cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  537.            }
  538.            // Arm 2: vertex to endpoint 2
  539.            hull() {
  540.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  541.                translate([h2x, h2y, 0])
  542.                    cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  543.            }
  544.        }
  545.        // Vertex bearing seat
  546.        bearing_seat();
  547.        // Endpoint 1 bearing seat
  548.        translate([h1x, h1y, 0]) bearing_seat();
  549.        // Endpoint 2 bearing seat
  550.        translate([h2x, h2y, 0]) bearing_seat();
  551.    }
  552. }
  553. """
  554.  
  555.  
  556. def _save_scad_and_stl(out_dir, params, ts):
  557.     """Generate per-piece .scad and .stl files."""
  558.     import subprocess
  559.  
  560.     crank = params["crank"]
  561.     B2 = params["B2"]
  562.     B3 = params["B3"]
  563.     B4 = params["B4"]
  564.     B5 = params["B5"]
  565.     B6 = params["B6_bar"]
  566.     B7 = params["B7"]
  567.     b6r = math.radians(params["B6_angle"])
  568.     b7r = math.radians(params["B7_angle"])
  569.  
  570.     # Bent coupler hole positions (relative to vertex J4, J1 along +X)
  571.     c_j1x, c_j1y = B2, 0.0
  572.     c_j6x = -B4 * math.cos(b6r)
  573.     c_j6y =  B4 * math.sin(b6r)
  574.  
  575.     # Bent output hole positions (relative to vertex J6, J7 along +X)
  576.     o_j7x, o_j7y = B5, 0.0
  577.     o_j8x = -B7 * math.cos(b7r)
  578.     o_j8y = -B7 * math.sin(b7r)
  579.  
  580.     # Find OpenSCAD binary
  581.     import shutil
  582.     openscad = shutil.which("openscad")
  583.     if openscad is None:
  584.         # macOS app bundle fallback
  585.         for p in ["/Applications/OpenSCAD.app/Contents/MacOS/OpenSCAD"]:
  586.             if os.path.isfile(p):
  587.                 openscad = p
  588.                 break
  589.     if openscad is None:
  590.         print("WARNING: openscad not found in PATH. .scad files saved but .stl skipped.")
  591.  
  592.     # Frame L-bracket: 3 bearing seats
  593.     frame_scad = _SCAD_PREAMBLE + f"""\
  594. FRAME_THICK = 6.0;
  595.  
  596. module frame_bracket() {{
  597.    j3x = -{FRAME_LOW_X:.2f};
  598.    j3y = -{FRAME_LOW_Y:.2f};
  599.    j5x = -{FRAME_HIGH_X:.2f};
  600.    j5y =  {FRAME_HIGH_Y:.2f};
  601.  
  602.    difference() {{
  603.        hull() {{
  604.            cylinder(h = FRAME_THICK, d = LINK_WIDTH, $fn = 48);
  605.            translate([j3x, j3y, 0])
  606.                cylinder(h = FRAME_THICK, d = LINK_WIDTH, $fn = 48);
  607.            translate([j5x, j5y, 0])
  608.                cylinder(h = FRAME_THICK, d = LINK_WIDTH, $fn = 48);
  609.        }}
  610.        // J0: bearing seat
  611.        bearing_seat(FRAME_THICK);
  612.        // J3: bearing seat
  613.        translate([j3x, j3y, 0]) bearing_seat(FRAME_THICK);
  614.        // J5: bearing seat
  615.        translate([j5x, j5y, 0]) bearing_seat(FRAME_THICK);
  616.    }}
  617. }}
  618.  
  619. frame_bracket();
  620. """
  621.  
  622.     # Build b5_b7 output piece(s) — split if B7 arm >= 105mm
  623.     SPLIT_THRESHOLD = 105.0
  624.     CUT_DIST = 40.0        # mm from J6 along B7 arm where the cut happens
  625.     PIN_DIA = 1.85         # 1.75mm filament + 0.1mm clearance
  626.     PIN_DEPTH = 10.5       # slightly over 10mm so pins seat fully
  627.     PIN_SPACING = 4.0      # mm from centerline to each pin
  628.  
  629.     if B7 >= SPLIT_THRESHOLD:
  630.         # Split the B7 arm into elbow + extension
  631.         b7_len = math.hypot(o_j8x, o_j8y)
  632.         dx = o_j8x / b7_len
  633.         dy = o_j8y / b7_len
  634.         # Perpendicular for pin placement
  635.         px, py = -dy, dx
  636.         # Cut point
  637.         cx, cy = dx * CUT_DIST, dy * CUT_DIST
  638.         # Pin positions at cut face — symmetric about bar centerline
  639.         # so the piece is reversible when flipped
  640.         p1x, p1y = cx + PIN_SPACING * px, cy + PIN_SPACING * py
  641.         p2x, p2y = cx - PIN_SPACING * px, cy - PIN_SPACING * py
  642.         # Pin holes start 1mm outside the cut face (into the void) and
  643.         # drill inward PIN_DEPTH+1mm, guaranteeing a clean opening
  644.         # Direction INTO elbow = away from J8 = (-dx, -dy)
  645.         pin_start_offset = 1.0  # mm outside cut face
  646.         ps1x = p1x + dx * pin_start_offset  # start point (outside)
  647.         ps1y = p1y + dy * pin_start_offset
  648.         ps2x = p2x + dx * pin_start_offset
  649.         ps2y = p2y + dy * pin_start_offset
  650.         # Arm angle for rotation
  651.         arm_ang = math.degrees(math.atan2(dy, dx))
  652.         pin_total = PIN_DEPTH + pin_start_offset
  653.         # Extension length
  654.         ext_len = b7_len - CUT_DIST
  655.  
  656.         # Elbow: full bent bar, then intersect to chop flat at cut line
  657.         # The cut plane passes through cut_point, perpendicular to the B7 arm
  658.         # We keep the J6/J7 side. Use a large cube on the "keep" side.
  659.         # Normal of cut plane points toward J8: (dx, dy). We keep the opposite side.
  660.         # Rotate cut cube so its face aligns with the cut perpendicular.
  661.         elbow_scad = _SCAD_PREAMBLE + f"""\
  662. // b5_b7 elbow: bent bar chopped flat at {CUT_DIST:.0f}mm along B7 arm
  663. difference() {{
  664.    intersection() {{
  665.        // Full bent bar (J7 -- J6 -- J8)
  666.        union() {{
  667.            hull() {{
  668.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  669.                translate([{o_j7x:.2f}, {o_j7y:.2f}, 0])
  670.                    cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  671.            }}
  672.            hull() {{
  673.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  674.                translate([{o_j8x:.2f}, {o_j8y:.2f}, 0])
  675.                    cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  676.            }}
  677.        }}
  678.        // Half-space: keep everything on J6 side of cut plane
  679.        translate([{cx:.2f}, {cy:.2f}, -1])
  680.            rotate([0, 0, {arm_ang:.2f}])
  681.            translate([-500, -500, 0])
  682.            cube([500, 1000, LINK_THICK + 2]);
  683.    }}
  684.    // J6 bearing seat (vertex)
  685.    bearing_seat();
  686.    // J7 bearing seat
  687.    translate([{o_j7x:.2f}, {o_j7y:.2f}, 0]) bearing_seat();
  688.    // Pin holes at cut face (start 1mm outside, drill into piece)
  689.    translate([{ps1x:.2f}, {ps1y:.2f}, LINK_THICK/2])
  690.        rotate([0, 0, {arm_ang + 180:.2f}])
  691.        rotate([0, 90, 0])
  692.        cylinder(h = {pin_total:.1f}, d = {PIN_DIA}, $fn = 16);
  693.    translate([{ps2x:.2f}, {ps2y:.2f}, LINK_THICK/2])
  694.        rotate([0, 0, {arm_ang + 180:.2f}])
  695.        rotate([0, 90, 0])
  696.        cylinder(h = {pin_total:.1f}, d = {PIN_DIA}, $fn = 16);
  697. }}
  698. """
  699.  
  700.         # Extension: full straight bar (cut_point to J8), chopped flat at cut end
  701.         # Oriented along X: x=0 is cut face, x=ext_len is J8
  702.         ext_scad = _SCAD_PREAMBLE + f"""\
  703. // b5_b7 extension: cut_point to J8 ({ext_len:.1f}mm)
  704. // x=0 is flat cut face, x={ext_len:.2f} is J8
  705. difference() {{
  706.    intersection() {{
  707.        // Full bar shape (rounded ends)
  708.        hull() {{
  709.            cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  710.            translate([{ext_len:.2f}, 0, 0])
  711.                cylinder(h = LINK_THICK, d = LINK_WIDTH, $fn = 48);
  712.        }}
  713.        // Chop: keep only x >= 0 (flat face at cut end)
  714.        translate([0, -LINK_WIDTH, -1])
  715.            cube([{ext_len + 20:.2f}, LINK_WIDTH * 2, LINK_THICK + 2]);
  716.    }}
  717.    // J8 bearing seat only (far end)
  718.    translate([{ext_len:.2f}, 0, 0]) bearing_seat();
  719.    // Pin holes at cut face (start 1mm outside at x=-1, drill into piece)
  720.    translate([-1, {PIN_SPACING}, LINK_THICK/2])
  721.        rotate([0, 90, 0])
  722.        cylinder(h = {pin_total:.1f}, d = {PIN_DIA}, $fn = 16);
  723.    translate([-1, {-PIN_SPACING}, LINK_THICK/2])
  724.        rotate([0, 90, 0])
  725.        cylinder(h = {pin_total:.1f}, d = {PIN_DIA}, $fn = 16);
  726. }}
  727. """
  728.         b5_b7_pieces = [
  729.             ("b5_b7_elbow", elbow_scad),
  730.             ("b5_b7_ext",   ext_scad),
  731.         ]
  732.     else:
  733.         b5_b7_pieces = [
  734.             ("b5_b7_output", _SCAD_PREAMBLE +
  735.                 f"bent_bar({o_j7x:.2f}, {o_j7y:.2f}, {o_j8x:.2f}, {o_j8y:.2f});"),
  736.         ]
  737.  
  738.     pieces = [
  739.         ("frame",       frame_scad),
  740.         ("b0_crank",    _SCAD_PREAMBLE + f"straight_bar({crank:.2f});"),
  741.         ("b2_b4_coupler", _SCAD_PREAMBLE +
  742.             f"bent_bar({c_j1x:.2f}, {c_j1y:.2f}, {c_j6x:.2f}, {c_j6y:.2f});"),
  743.         ("b3_rocker",   _SCAD_PREAMBLE + f"straight_bar({B3:.2f});"),
  744.         ("b6_rocker",   _SCAD_PREAMBLE + f"straight_bar({B6:.2f});"),
  745.     ] + b5_b7_pieces
  746.  
  747.     for name, scad_content in pieces:
  748.         scad_path = os.path.join(out_dir, f"{ts}_{name}.scad")
  749.         stl_path  = os.path.join(out_dir, f"{ts}_{name}.stl")
  750.  
  751.         with open(scad_path, 'w') as f:
  752.             f.write(scad_content)
  753.  
  754.         if openscad:
  755.             try:
  756.                 result = subprocess.run(
  757.                     [openscad, "-o", stl_path, scad_path],
  758.                     timeout=120, capture_output=True, text=True
  759.                 )
  760.                 if result.returncode != 0:
  761.                     print(f"OpenSCAD error ({name}): {result.stderr[:200]}")
  762.             except FileNotFoundError:
  763.                 print(f"OpenSCAD not found at: {openscad}")
  764.                 openscad = None  # stop trying for remaining pieces
  765.             except Exception as e:
  766.                 print(f"OpenSCAD failed ({name}): {e}")
  767.         elif not openscad:
  768.             pass  # already warned or not installed
  769.  
  770.  
  771. def _save_profile_png(png_path, foot_path, ground_pts,
  772.                       step_length_mm, step_height_mm, stride_px,
  773.                       mn_x, mx_x, mn_y, mx_y, params, ts, max_torque):
  774.     """Render step profile to a PNG using pygame."""
  775.     W, H = 800, 600
  776.     BG_PNG = (15, 18, 25)
  777.     surf = pygame.Surface((W, H))
  778.     surf.fill(BG_PNG)
  779.  
  780.     # Use pygame's built-in default font with background color for clean PNG export
  781.     pfont = pygame.font.Font(None, 24)
  782.     sfont = pygame.font.Font(None, 20)
  783.  
  784.     def txt(font, text, color, pos):
  785.         surf.blit(font.render(text, True, color, BG_PNG), pos)
  786.  
  787.     # Title
  788.     txt(pfont, f"Klann Step Profile  -  {ts}", (140, 170, 210), (20, 15))
  789.  
  790.     # Draw foot path centered in a box
  791.     margin = 80
  792.     plot_x, plot_y = margin, 60
  793.     plot_w, plot_h = W - 2 * margin, 280
  794.  
  795.     if not foot_path or (mx_x - mn_x) < 1 or (mx_y - mn_y) < 1:
  796.         txt(sfont, "No valid foot path", (200, 80, 80), (plot_x, plot_y + 50))
  797.         pygame.image.save(surf, png_path)
  798.         return
  799.  
  800.     path_w = mx_x - mn_x
  801.     path_h = mx_y - mn_y
  802.     scale = min(plot_w / path_w, plot_h / path_h) * 0.80
  803.     cx = plot_x + plot_w / 2
  804.     cy = plot_y + plot_h / 2
  805.     center_x = (mn_x + mx_x) / 2
  806.     center_y = (mn_y + mx_y) / 2
  807.  
  808.     def pt(p):
  809.         return (int(cx + (p[0] - center_x) * scale),
  810.                 int(cy + (p[1] - center_y) * scale))
  811.  
  812.     # Grid lines
  813.     for gx in range(plot_x, plot_x + plot_w, 40):
  814.         pygame.draw.line(surf, (25, 30, 40), (gx, plot_y), (gx, plot_y + plot_h), 1)
  815.     for gy in range(plot_y, plot_y + plot_h, 40):
  816.         pygame.draw.line(surf, (25, 30, 40), (plot_x, gy), (plot_x + plot_w, gy), 1)
  817.  
  818.     # Draw foot path (don't close loop — actual path is a time series, not a closed curve)
  819.     n = len(foot_path)
  820.     for i in range(1, n):
  821.         f = i / n
  822.         col = (int(220 * f), int(160 * f), int(40 * f))
  823.         p1 = pt(foot_path[i - 1])
  824.         p2 = pt(foot_path[i])
  825.         pygame.draw.line(surf, col, p1, p2, 2)
  826.  
  827.     # Highlight ground contact
  828.     if ground_pts:
  829.         g_sorted = sorted(ground_pts, key=lambda p: p[0])
  830.         for i in range(1, len(g_sorted)):
  831.             p1 = pt(g_sorted[i - 1])
  832.             p2 = pt(g_sorted[i])
  833.             pygame.draw.line(surf, (100, 220, 100), p1, p2, 4)
  834.  
  835.     # Dimension lines
  836.     # Step length (horizontal, along ground)
  837.     gnd_screen_y = cy + (mx_y - center_y) * scale
  838.     dim_y = int(gnd_screen_y + 25)
  839.     if ground_pts:
  840.         gxs = [p[0] for p in ground_pts]
  841.         gl = pt((min(gxs), mx_y))
  842.         gr = pt((max(gxs), mx_y))
  843.     else:
  844.         gl = pt((mn_x, mx_y))
  845.         gr = pt((mx_x, mx_y))
  846.     pygame.draw.line(surf, (100, 220, 100), (gl[0], dim_y), (gr[0], dim_y), 1)
  847.     for dx in [gl[0], gr[0]]:
  848.         pygame.draw.line(surf, (100, 220, 100), (dx, dim_y - 5), (dx, dim_y + 5), 1)
  849.         pygame.draw.line(surf, (50, 100, 50), (dx, int(gnd_screen_y)), (dx, dim_y), 1)
  850.     length_label = sfont.render(f"step length: {step_length_mm:.1f} mm", True, (100, 220, 100), BG_PNG)
  851.     surf.blit(length_label, ((gl[0] + gr[0]) // 2 - length_label.get_width() // 2, dim_y + 8))
  852.  
  853.     # Step height (vertical)
  854.     top_screen_y = cy + (mn_y - center_y) * scale
  855.     bot_screen_y = gnd_screen_y
  856.     dim_x = int(cx + (mx_x - center_x) * scale + 25)
  857.     pygame.draw.line(surf, (220, 160, 40), (dim_x, int(top_screen_y)), (dim_x, int(bot_screen_y)), 1)
  858.     for dy in [int(top_screen_y), int(bot_screen_y)]:
  859.         pygame.draw.line(surf, (220, 160, 40), (dim_x - 5, dy), (dim_x + 5, dy), 1)
  860.     height_label = sfont.render(f"step height: {step_height_mm:.1f} mm", True, (220, 160, 40), BG_PNG)
  861.     surf.blit(height_label, (dim_x + 8, int((top_screen_y + bot_screen_y) / 2) - 6))
  862.  
  863.     # Stats text block
  864.     ty = plot_y + plot_h + 30
  865.     stats = [
  866.         f"Step Length:  {step_length_mm:.1f} mm",
  867.         f"Step Height:  {step_height_mm:.1f} mm",
  868.         f"Stride Width: {stride_px:.1f} mm",
  869.         f"Max Torque:   {max_torque:.3f} Nm (3 cyc)",
  870.         f"Recommended:  {max_torque * 5.7:.2f} Nm",
  871.     ]
  872.     colors = [(100, 220, 100), (220, 160, 40), (140, 170, 210), (180, 140, 220), (220, 170, 80)]
  873.     for line, col in zip(stats, colors):
  874.         txt(sfont, line, col, (40, ty))
  875.         ty += 22
  876.  
  877.     # Parameters block
  878.     ty += 8
  879.     txt(pfont, "Parameters", (120, 140, 170), (40, ty))
  880.     ty += 24
  881.     param_lines = [
  882.         f"crank={params['crank']:.1f}  B2={params['B2']:.1f}  B3={params['B3']:.1f}  B4={params['B4']:.1f}",
  883.         f"B5={params['B5']:.1f}  B6={params['B6_bar']:.1f}  B7={params['B7']:.1f}",
  884.         f"B6_angle={params['B6_angle']:.1f}  B7_angle={params['B7_angle']:.1f}",
  885.     ]
  886.     for line in param_lines:
  887.         txt(sfont, line, (100, 110, 125), (40, ty))
  888.         ty += 20
  889.  
  890.     pygame.image.save(surf, png_path)
  891.  
  892.  
  893. # ============================================================
  894. # Drawing helpers
  895. # ============================================================
  896. def ipx(pt, ox, oy):
  897.     return (int(pt[0]+ox), int(pt[1]+oy))
  898.  
  899.  
  900. def tl(surf, col, p1, p2, w):
  901.     pygame.draw.line(surf, col, (int(p1[0]),int(p1[1])), (int(p2[0]),int(p2[1])), w)
  902.  
  903.  
  904. def jnt(surf, col, pos, r):
  905.     pygame.draw.circle(surf, col, (int(pos[0]),int(pos[1])), r)
  906.     if r > 3:
  907.         pygame.draw.circle(surf, (220,225,235), (int(pos[0]),int(pos[1])), r-2, 1)
  908.  
  909.  
  910. def dc(col, f):
  911.     return tuple(min(255,int(v*f)) for v in col)
  912.  
  913.  
  914. def draw_leg(surf, j, ox, oy, near=True, highlight_segs=None, blink_alpha=1.0):
  915.     """Draw one Klann leg with all linkage bars.
  916.    highlight_segs: list of (key1, key2) segment pairs to blink safety orange.
  917.    blink_alpha: 0.0-1.0 pulse for the blink effect.
  918.    """
  919.     if j is None:
  920.         return
  921.     f = 1.0 if near else 0.65
  922.     lc = dc(LEG_COLOR,   f)
  923.     cc = dc(CRANK_COLOR, f)
  924.     lk = dc(LINK_COLOR,  f)
  925.     fc = dc(FOOT_COLOR,  f)
  926.     jc = dc(JOINT_COLOR, f)
  927.     tw = 1 if near else 0
  928.  
  929.     # Build set of highlighted segments for fast lookup
  930.     hl = set()
  931.     if highlight_segs and near:
  932.         for a, b in highlight_segs:
  933.             hl.add((a, b))
  934.             hl.add((b, a))
  935.  
  936.     def is_hl(k1, k2):
  937.         return (k1, k2) in hl
  938.  
  939.     def hl_col(base_col, k1, k2, extra_w=0):
  940.         """Return color and width boost for a segment, blinking if highlighted."""
  941.         if is_hl(k1, k2):
  942.             # Lerp between base color and safety orange based on blink_alpha
  943.             r = int(base_col[0] + (SAFETY_ORANGE[0] - base_col[0]) * blink_alpha)
  944.             g = int(base_col[1] + (SAFETY_ORANGE[1] - base_col[1]) * blink_alpha)
  945.             b = int(base_col[2] + (SAFETY_ORANGE[2] - base_col[2]) * blink_alpha)
  946.             return (r, g, b), extra_w + 2
  947.         return base_col, extra_w
  948.  
  949.     def p(key): return ipx(j[key], ox, oy)
  950.  
  951.     # Frame struts (fixed pivot connections)
  952.     tl(surf, dc(BODY_DARK, f), p("O"), p("P_low"), 3+tw)
  953.     tl(surf, dc(BODY_DARK, f), p("O"), p("P_high"), 3+tw)
  954.     # Crank O→C (J0→J1)
  955.     col, ew = hl_col(cc, "O", "C")
  956.     tl(surf, col, p("O"), p("C"), 3+tw+ew)
  957.     # B2: C→D (J1→J4)
  958.     col, ew = hl_col(lk, "C", "D")
  959.     tl(surf, col, p("C"), p("D"), 4+tw+ew)
  960.     # B3: P_low→D (J3→J4)
  961.     col, ew = hl_col(lk, "P_low", "D")
  962.     tl(surf, col, p("P_low"), p("D"), 3+tw+ew)
  963.     # B4: D→E rigid bent bar (J4→J6)
  964.     col, ew = hl_col(lc, "D", "E")
  965.     tl(surf, col, p("D"), p("E"), 5+tw+ew)
  966.     # B6: P_high→G (J5→J7)
  967.     col, ew = hl_col(lk, "P_high", "G")
  968.     tl(surf, col, p("P_high"), p("G"), 3+tw+ew)
  969.     # B5: E→G (J6→J7)
  970.     col, ew = hl_col(lk, "E", "G")
  971.     tl(surf, col, p("E"), p("G"), 3+tw+ew)
  972.     # B7: E→F rigid bent bar to foot (J6→J8)
  973.     col, ew = hl_col(lc, "E", "F")
  974.     tl(surf, col, p("E"), p("F"), 6+tw+ew)
  975.  
  976.     # Foot claws at F (J8)
  977.     ft = j["F"]
  978.     claw_len = 12 if near else 8
  979.     for sign in [-1, 1]:
  980.         cx2 = ft[0] + sign * 5
  981.         cy2 = ft[1]
  982.         tx2 = cx2 + sign * 3
  983.         ty2 = cy2 + claw_len * 0.7
  984.         tl(surf, fc,
  985.            (int(cx2+ox), int(cy2+oy)),
  986.            (int(tx2+ox), int(ty2+oy)), 2+tw)
  987.  
  988.     # Joints — highlight endpoints of highlighted segments
  989.     jr = 5 if near else 3
  990.     for key, r in [("O",jr+2),("P_low",jr),("P_high",jr),("C",jr-1),
  991.                    ("D",jr+1),("E",jr),("G",jr),("F",jr+1)]:
  992.         jcol = jc
  993.         if any(key in seg for seg in hl):
  994.             cr = int(jc[0] + (SAFETY_ORANGE[0] - jc[0]) * blink_alpha)
  995.             cg = int(jc[1] + (SAFETY_ORANGE[1] - jc[1]) * blink_alpha)
  996.             cb = int(jc[2] + (SAFETY_ORANGE[2] - jc[2]) * blink_alpha)
  997.             jcol = (cr, cg, cb)
  998.         jnt(surf, jcol, p(key), r)
  999.  
  1000.  
  1001. def draw_body(surf, front_x, rear_x, body_y):
  1002.     """Draw quadruped body."""
  1003.     bw = int(front_x - rear_x)
  1004.     bh = 60
  1005.     bx = int(rear_x)
  1006.     by = int(body_y) - bh - 10
  1007.  
  1008.     pygame.draw.rect(surf, (20,25,35), (bx+3,by+3,bw,bh), border_radius=6)
  1009.     pygame.draw.rect(surf, BODY_COLOR, (bx,by,bw,bh), border_radius=5)
  1010.     pygame.draw.rect(surf, BODY_DARK,  (bx,by,bw,bh), 2, border_radius=5)
  1011.  
  1012.     strip_w = bw - 30
  1013.     if strip_w > 20:
  1014.         pygame.draw.rect(surf, (38,52,72), (bx+15, by+12, strip_w, 18), border_radius=3)
  1015.         vp_count = max(1, strip_w // 40)
  1016.         vp_w = min(22, (strip_w - 10) // vp_count - 4)
  1017.         for i in range(vp_count):
  1018.             vx = bx + 19 + i * (vp_w + 6)
  1019.             pygame.draw.rect(surf, (55,95,125), (vx, by+14, vp_w, 14), border_radius=2)
  1020.  
  1021.     pygame.draw.rect(surf, BODY_DARK, (bx-12, by+14, 12, 30), border_radius=3)
  1022.     pygame.draw.rect(surf, BODY_DARK, (bx+bw, by+14, 12, 30), border_radius=3)
  1023.     pygame.draw.rect(surf, BODY_DARK, (bx+bw-5, by+bh-18, 22, 12), border_radius=2)
  1024.     pygame.draw.rect(surf, (50,58,68), (bx+bw+15, by+bh-16, 10, 4), border_radius=1)
  1025.     pygame.draw.rect(surf, (50,58,68), (bx+bw+15, by+bh-10, 10, 4), border_radius=1)
  1026.     pygame.draw.rect(surf, BODY_DARK, (bx+20, by-5, bw-40, 7), border_radius=2)
  1027.     cx = bx + bw // 2
  1028.     pygame.draw.rect(surf, (62,70,82), (cx-14, by+bh, 28, 14), border_radius=3)
  1029.  
  1030.  
  1031. def draw_ground(surf, gy):
  1032.     pygame.draw.line(surf, GROUND_COLOR, (0,gy), (WIDTH,gy), 2)
  1033.     for x in range(0, WIDTH, 18):
  1034.         pygame.draw.line(surf, (30,38,48), (x,gy), (x+9,gy+7), 1)
  1035.  
  1036.  
  1037. def draw_trace(surf, trace, color):
  1038.     n = len(trace)
  1039.     if n < 2:
  1040.         return
  1041.     for i in range(1, n):
  1042.         f = i / n
  1043.         dim = tuple(int(c * f) for c in color)
  1044.         p1 = (int(trace[i-1][0]), int(trace[i-1][1]))
  1045.         p2 = (int(trace[i][0]), int(trace[i][1]))
  1046.         pygame.draw.line(surf, dim, p1, p2, 1)
  1047.  
  1048.  
  1049. # ============================================================
  1050. # Slider UI
  1051. # ============================================================
  1052. def draw_slider(surf, idx, val):
  1053.     key, label, mn, mx = SLIDERS[idx]
  1054.     sy = SLIDER_START + idx * SLIDER_STEP
  1055.     surf.blit(font_sm.render(label, True, TEXT_DIM), (SLIDER_X, sy-13))
  1056.     if "angle" in key:
  1057.         disp = f"{val:.1f}°"
  1058.     elif key == "speed":
  1059.         disp = f"{val:.1f}"
  1060.     else:
  1061.         disp = f"{val:.1f}"
  1062.     surf.blit(font_sm.render(disp, True, TEXT_COLOR), (SLIDER_X+SLIDER_W+8, sy-4))
  1063.     pygame.draw.rect(surf, SLIDER_BG, (SLIDER_X, sy, SLIDER_W, SLIDER_H), border_radius=3)
  1064.     fw = int((val-mn)/(mx-mn)*SLIDER_W)
  1065.     if fw > 0:
  1066.         pygame.draw.rect(surf, SLIDER_FG, (SLIDER_X, sy, fw, SLIDER_H), border_radius=3)
  1067.     hx = SLIDER_X + fw
  1068.     pygame.draw.circle(surf, SLIDER_HAND, (hx, sy+SLIDER_H//2), 8)
  1069.     pygame.draw.circle(surf, (220,235,250), (hx, sy+SLIDER_H//2), 4)
  1070.  
  1071.  
  1072. def slider_rect(idx):
  1073.     sy = SLIDER_START + idx * SLIDER_STEP
  1074.     return pygame.Rect(SLIDER_X-8, sy-8, SLIDER_W+16, SLIDER_H+16)
  1075.  
  1076.  
  1077. def slider_val(idx, mx_pos):
  1078.     key, label, mn, mx = SLIDERS[idx]
  1079.     t = max(0.0, min(1.0, (mx_pos - SLIDER_X) / SLIDER_W))
  1080.     return mn + t*(mx-mn)
  1081.  
  1082.  
  1083. # ============================================================
  1084. # Layout
  1085. # ============================================================
  1086. GROUND_Y = 650
  1087. BODY_CX  = 560
  1088. HIP_SHOULDER_PX = 16.0 * PX_PER_INCH  # 320 px
  1089.  
  1090. angle    = 0.0
  1091. running  = True
  1092.  
  1093. while running:
  1094.     dt = clock.tick(60) / 1000.0
  1095.  
  1096.     for event in pygame.event.get():
  1097.         if event.type == pygame.QUIT:
  1098.             running = False
  1099.         if event.type == pygame.KEYDOWN:
  1100.             if event.key == pygame.K_ESCAPE:
  1101.                 running = False
  1102.             if event.key == pygame.K_t:
  1103.                 show_traces = not show_traces
  1104.                 trace_front.clear()
  1105.                 trace_rear.clear()
  1106.  
  1107.         if event.type == pygame.MOUSEBUTTONDOWN:
  1108.             for i in range(len(SLIDERS)):
  1109.                 if slider_rect(i).collidepoint(event.pos):
  1110.                     dragging = i
  1111.                     params[SLIDERS[i][0]] = slider_val(i, event.pos[0])
  1112.             btn_y = SLIDER_START+len(SLIDERS)*SLIDER_STEP+6
  1113.             reset_r = pygame.Rect(SLIDER_X, btn_y, 55, 22)
  1114.             reset2_r = pygame.Rect(SLIDER_X + 61, btn_y, 55, 22)
  1115.             reset3_r = pygame.Rect(SLIDER_X + 122, btn_y, 55, 22)
  1116.             for btn_rect, btn_params in [(reset_r, DEFAULT_PARAMS), (reset2_r, PRESET2_PARAMS), (reset3_r, PRESET3_PARAMS)]:
  1117.                 if btn_rect.collidepoint(event.pos):
  1118.                     params = dict(btn_params)
  1119.                     trace_front.clear()
  1120.                     trace_rear.clear()
  1121.                     actual_foot_path.clear()
  1122.                     torque_history.clear()
  1123.                     prev_foot_pos = None
  1124.                     prev_foot_vel = None
  1125.                     max_torque_3cyc = 0.0
  1126.             save_btn_y = btn_y + 30
  1127.             save_r = pygame.Rect(SLIDER_X, save_btn_y, 177, 22)
  1128.             if save_r.collidepoint(event.pos):
  1129.                 filepath = save_geometry(params, actual_foot_path, max_torque_3cyc)
  1130.                 base = os.path.basename(filepath).replace('.json', '')
  1131.                 save_flash_msg = f"{base} json+png+6x scad/stl"
  1132.                 save_flash_timer = 120  # frames to show confirmation
  1133.  
  1134.         if event.type == pygame.MOUSEBUTTONUP:
  1135.             dragging = None
  1136.  
  1137.         if event.type == pygame.MOUSEMOTION and dragging is not None:
  1138.             params[SLIDERS[dragging][0]] = slider_val(dragging, event.pos[0])
  1139.  
  1140.     walk_spd = params["speed"] / 1000.0
  1141.     angle += walk_spd
  1142.     steps_per_rot = max(60, int(2.0 * math.pi / max(walk_spd, 0.001)))
  1143.  
  1144.     linkage_p = {k: v for k, v in params.items() if k != "speed"}
  1145.  
  1146.     # Solve all 4 legs (diagonal gait: phase A and phase B at pi offset)
  1147.     ja = solve_klann(angle, linkage_p)
  1148.     jb = solve_klann(angle + math.pi, linkage_p)
  1149.  
  1150.     if ja is None or jb is None:
  1151.         angle += 0.05
  1152.         continue
  1153.  
  1154.     # Ground pinning: lowest foot F touches ground
  1155.     foot_ys = [ja["F"][1], jb["F"][1]]
  1156.     max_foot_y = max(foot_ys)
  1157.     oy_base = GROUND_Y - max_foot_y
  1158.  
  1159.     front_hip_x = BODY_CX + HIP_SHOULDER_PX / 2
  1160.     rear_hip_x  = BODY_CX - HIP_SHOULDER_PX / 2
  1161.  
  1162.     a_stance = ja["F"][1] >= jb["F"][1]
  1163.  
  1164.     def foot_screen(joints, hip_x, oy):
  1165.         ft = joints["F"]
  1166.         return (ft[0] + hip_x, ft[1] + oy)
  1167.  
  1168.     ft_front_near = foot_screen(ja, front_hip_x, oy_base)
  1169.     ft_rear_near  = foot_screen(jb, rear_hip_x, oy_base)
  1170.  
  1171.     if show_traces:
  1172.         trace_front.append(ft_front_near)
  1173.         trace_rear.append(ft_rear_near)
  1174.         if len(trace_front) > TRACE_LEN:
  1175.             trace_front.pop(0)
  1176.         if len(trace_rear) > TRACE_LEN:
  1177.             trace_rear.pop(0)
  1178.  
  1179.     # Foot path metrics
  1180.     foot_path = compute_foot_path(linkage_p)
  1181.     stride_px, height_px, _, _, _, _, ground_px, ground_pts, foot_strike = path_metrics(foot_path)
  1182.     # Linkage coordinates are mm — convert to inches properly
  1183.     MM_PER_INCH = 25.4
  1184.     stride_mm = stride_px   # these are already mm
  1185.     height_mm = height_px
  1186.     ground_mm = ground_px
  1187.     stride_in = stride_mm / MM_PER_INCH
  1188.     height_in = height_mm / MM_PER_INCH
  1189.     ground_in = ground_mm / MM_PER_INCH
  1190.  
  1191.     # --- Torque estimation ---
  1192.     # Convert pixel positions to meters for physics
  1193.     crank_m = params["crank"] / PX_PER_M
  1194.     # Foot position relative to crank center (in meters)
  1195.     foot_rel_x = ja["F"][0] / PX_PER_M   # relative to O, screen coords
  1196.     foot_rel_y = ja["F"][1] / PX_PER_M
  1197.  
  1198.     # Numerical velocity and acceleration of foot (m/s, m/s^2)
  1199.     cur_foot = (ja["F"][0] / PX_PER_M, ja["F"][1] / PX_PER_M)
  1200.     foot_vel = (0.0, 0.0)
  1201.     foot_accel = (0.0, 0.0)
  1202.     effective_dt = dt if dt > 0 else 1.0/60.0
  1203.     if prev_foot_pos is not None:
  1204.         foot_vel = ((cur_foot[0] - prev_foot_pos[0]) / effective_dt,
  1205.                     (cur_foot[1] - prev_foot_pos[1]) / effective_dt)
  1206.     if prev_foot_vel is not None:
  1207.         foot_accel = ((foot_vel[0] - prev_foot_vel[0]) / effective_dt,
  1208.                       (foot_vel[1] - prev_foot_vel[1]) / effective_dt)
  1209.     prev_foot_pos = cur_foot
  1210.     prev_foot_vel = foot_vel
  1211.  
  1212.     # Torque components:
  1213.     # 1) Gravity: body weight distributed across 2 stance legs.
  1214.     #    Torque = (load per leg) * g * horizontal offset of foot from crank * (crank_m / foot_lever)
  1215.     #    Simplified: use virtual work — crank torque = F_foot · (d_foot/d_theta) projected
  1216.     #    We use the simpler static approach: T_gravity = (mass_share * g) * |foot_horizontal_offset|
  1217.     #    scaled by mechanical advantage (crank / foot_lever_arm)
  1218.     foot_lever = math.hypot(foot_rel_x, foot_rel_y)
  1219.     foot_lever = max(foot_lever, 0.001)
  1220.     # Each stance leg supports half the total weight (diagonal gait = 2 legs on ground)
  1221.     weight_per_leg = (TOTAL_MASS * GRAVITY) / 2.0
  1222.     # Gravity torque on crank: T = F * r_foot * (r_crank / r_foot) via virtual work
  1223.     # More precisely: T_crank = F_vertical * dx_foot/dtheta
  1224.     # Approximate dx_foot/dtheta ≈ foot_vel_x / omega, omega = walk_spd / dt_per_frame...
  1225.     # Simpler static model: T = weight * horizontal_arm * (crank/lever) ratio
  1226.     horizontal_arm_m = abs(foot_rel_x)
  1227.     mech_advantage = crank_m / foot_lever if foot_lever > 0 else 1.0
  1228.     torque_gravity = weight_per_leg * horizontal_arm_m * mech_advantage
  1229.  
  1230.     # 2) Joint friction: sum over ~7 revolute joints per leg, each with bearing friction
  1231.     #    T_friction_per_joint ≈ μ * F_joint * r_pin
  1232.     #    Approximate F_joint ≈ weight_per_leg for loaded joints
  1233.     n_loaded_joints = 5  # joints under significant load in stance
  1234.     torque_friction = n_loaded_joints * MU_JOINT * weight_per_leg * JOINT_RAD_M * (1.0 / mech_advantage)
  1235.  
  1236.     # 3) Inertia: T = I_leg * alpha, approximated as m_leg * a_foot * crank_m
  1237.     accel_mag = math.hypot(foot_accel[0], foot_accel[1])
  1238.     torque_inertia = LEG_MASS * accel_mag * crank_m
  1239.  
  1240.     # Total instantaneous crank torque (Nm) for one leg
  1241.     # In diagonal gait, 2 legs are in stance simultaneously, but they share one crank
  1242.     # so total crank torque ≈ 2 * single leg torque
  1243.     instant_torque = 2.0 * (torque_gravity + torque_friction) + torque_inertia
  1244.  
  1245.     # Clamp spikes from numerical differentiation on first few frames
  1246.     instant_torque = min(instant_torque, 5.0)
  1247.  
  1248.     torque_history.append(instant_torque)
  1249.     while len(torque_history) > steps_per_rot * 3:
  1250.         torque_history.pop(0)
  1251.     max_torque_3cyc = max(torque_history) if torque_history else 0.0
  1252.  
  1253.     # --- Draw ---
  1254.     screen.fill(BG)
  1255.  
  1256.     for gx in range(0, WIDTH, 40):
  1257.         pygame.draw.line(screen, GRID_COLOR, (gx,0), (gx,HEIGHT), 1)
  1258.     for gy in range(0, HEIGHT, 40):
  1259.         pygame.draw.line(screen, GRID_COLOR, (0,gy), (WIDTH,gy), 1)
  1260.  
  1261.     draw_ground(screen, GROUND_Y)
  1262.  
  1263.     shadow_w = int(HIP_SHOULDER_PX) + 60
  1264.     pygame.draw.ellipse(screen, (22,28,38),
  1265.                         (int(BODY_CX - shadow_w//2), GROUND_Y-5, shadow_w, 12))
  1266.  
  1267.     # Collect actual foot path of front-near leg (keep 3 full rotations)
  1268.     actual_foot_path.append(ft_front_near)
  1269.     while len(actual_foot_path) > steps_per_rot * 3:
  1270.         actual_foot_path.pop(0)
  1271.  
  1272.     if show_traces:
  1273.         draw_trace(screen, trace_front, TRACE_NEAR)
  1274.         draw_trace(screen, trace_rear, TRACE_FAR)
  1275.  
  1276.     # Compute blink for highlighted segment
  1277.     hl_segs = None
  1278.     blink_alpha = 0.0
  1279.     if dragging is not None:
  1280.         param_key = SLIDERS[dragging][0]
  1281.         hl_segs = SLIDER_SEGMENTS.get(param_key, [])
  1282.         if hl_segs:
  1283.             blink_alpha = 0.5 + 0.5 * math.sin(angle * 12.0)
  1284.  
  1285.     # Far-side legs
  1286.     draw_leg(screen, jb, front_hip_x, oy_base, near=False)
  1287.     draw_leg(screen, ja, rear_hip_x, oy_base, near=False)
  1288.  
  1289.     # Body
  1290.     draw_body(screen, front_hip_x, rear_hip_x, oy_base)
  1291.  
  1292.     # Near-side legs (with highlight)
  1293.     draw_leg(screen, ja, front_hip_x, oy_base, near=True,
  1294.              highlight_segs=hl_segs, blink_alpha=blink_alpha)
  1295.     draw_leg(screen, jb, rear_hip_x, oy_base, near=True,
  1296.              highlight_segs=hl_segs, blink_alpha=blink_alpha)
  1297.  
  1298.     # --- UI Panel ---
  1299.     panel_h = len(SLIDERS) * SLIDER_STEP + 100
  1300.     panel = pygame.Surface((250, panel_h), pygame.SRCALPHA)
  1301.     panel.fill((18, 22, 32, 210))
  1302.     screen.blit(panel, (12, SLIDER_START - 30))
  1303.  
  1304.     screen.blit(font_bold.render("KLANN PATENT LINKAGE", True, (130,165,200)),
  1305.                 (SLIDER_X, SLIDER_START - 22))
  1306.  
  1307.     for i in range(len(SLIDERS)):
  1308.         draw_slider(screen, i, params[SLIDERS[i][0]])
  1309.  
  1310.     btn_y = SLIDER_START+len(SLIDERS)*SLIDER_STEP+6
  1311.     for bx, bw, label, prm in [
  1312.         (SLIDER_X,       55, "RST",  DEFAULT_PARAMS),
  1313.         (SLIDER_X + 61,  55, "RST2", PRESET2_PARAMS),
  1314.         (SLIDER_X + 122, 55, "RST3", PRESET3_PARAMS),
  1315.     ]:
  1316.         br = pygame.Rect(bx, btn_y, bw, 22)
  1317.         pygame.draw.rect(screen, (45,55,70),  br, border_radius=4)
  1318.         pygame.draw.rect(screen, (75,95,115), br, 1, border_radius=4)
  1319.         lbl_w = font_sm.size(label)[0]
  1320.         screen.blit(font_sm.render(label, True, TEXT_COLOR), (bx + (bw - lbl_w)//2, btn_y+5))
  1321.  
  1322.     # Save button
  1323.     save_btn_y = btn_y + 30
  1324.     save_r = pygame.Rect(SLIDER_X, save_btn_y, 177, 22)
  1325.     pygame.draw.rect(screen, (40, 55, 45), save_r, border_radius=4)
  1326.     pygame.draw.rect(screen, (70, 110, 80), save_r, 1, border_radius=4)
  1327.     lbl_w = font_sm.size("SAVE JSON")[0]
  1328.     screen.blit(font_sm.render("SAVE JSON", True, (140, 200, 150)),
  1329.                 (SLIDER_X + (177 - lbl_w)//2, save_btn_y+5))
  1330.     if save_flash_timer > 0:
  1331.         save_flash_timer -= 1
  1332.         flash_alpha = min(255, save_flash_timer * 4)
  1333.         flash_col = (100, 220, 120) if save_flash_timer > 20 else (80, 160, 100)
  1334.         screen.blit(font_sm.render(save_flash_msg, True, flash_col),
  1335.                     (SLIDER_X, save_btn_y + 25))
  1336.  
  1337.     # --- Header ---
  1338.     assembled_kg = TOTAL_MASS * 2.0  # realistic assembled weight (2x bare)
  1339.     screen.blit(font_bold.render("AT-AT QUADRUPED  //  KLANN 6-BAR PATENT", True, (115,145,175)),
  1340.                 (300, 18))
  1341.     screen.blit(font_sm.render(
  1342.         f"stride: {stride_mm:.0f}mm ({stride_in:.1f}\")  "
  1343.         f"clearance: {height_mm:.0f}mm ({height_in:.1f}\")  "
  1344.         f"ground: {ground_mm:.0f}mm ({ground_in:.1f}\")",
  1345.         True, (65,78,92)), (300, 38))
  1346.     screen.blit(font_sm.render(
  1347.         f"bare: {TOTAL_MASS:.1f}kg  assembled: ~{assembled_kg:.1f}kg  "
  1348.         f"legs: {LEG_MASS*1000:.0f}g x4  body: {BODY_MASS*1000:.0f}g",
  1349.         True, (55,68,82)), (300, 54))
  1350.  
  1351.     pct = (angle % (2*math.pi)) / (2*math.pi)
  1352.     pygame.draw.rect(screen, SLIDER_BG,    (300, 72, 200, 6), border_radius=3)
  1353.     pygame.draw.rect(screen, (75,115,95),  (300, 72, int(200*pct), 6), border_radius=3)
  1354.     screen.blit(font_sm.render("cycle", True, TEXT_DIM), (508, 68))
  1355.  
  1356.     diag = "FR+RL stance" if a_stance else "FL+RR stance"
  1357.     screen.blit(font_sm.render(diag, True, (100,160,120)), (300, 84))
  1358.     screen.blit(font_sm.render(
  1359.         f"speed: {params['speed']:.0f}   [T] traces  [ESC] quit",
  1360.         True, TEXT_DIM), (300, 98))
  1361.  
  1362.     # Foot strike warning
  1363.     if foot_strike:
  1364.         warn_text = "FOOT STRIKE — swing path hits ground"
  1365.         warn_surf = font_bold.render(warn_text, True, (220, 60, 60))
  1366.         pulse = int(180 + 75 * math.sin(angle * 8.0))
  1367.         warn_bg = pygame.Surface((warn_surf.get_width()+16, warn_surf.get_height()+8), pygame.SRCALPHA)
  1368.         warn_bg.fill((60, 20, 20, pulse))
  1369.         wx = WIDTH // 2 - warn_bg.get_width() // 2
  1370.         screen.blit(warn_bg, (wx, 108))
  1371.         screen.blit(warn_surf, (wx + 8, 112))
  1372.  
  1373.     # --- Foot path mini diagram ---
  1374.     if foot_path:
  1375.         _, _, mn_x, mx_x, mn_y, mx_y, _, _, _ = path_metrics(foot_path)
  1376.         fp_cx, fp_cy = WIDTH - 120, 140
  1377.         path_w = mx_x - mn_x
  1378.         path_h = mx_y - mn_y
  1379.         if path_w > 0 and path_h > 0:
  1380.             fp_scale = min(160.0 / path_w, 120.0 / path_h, 1.0)
  1381.         else:
  1382.             fp_scale = 0.5
  1383.         center_x = (mn_x + mx_x) / 2
  1384.         center_y = (mn_y + mx_y) / 2
  1385.  
  1386.         for i in range(1, len(foot_path)):
  1387.             x1 = fp_cx + (foot_path[i-1][0] - center_x) * fp_scale
  1388.             y1 = fp_cy + (foot_path[i-1][1] - center_y) * fp_scale
  1389.             x2 = fp_cx + (foot_path[i][0] - center_x) * fp_scale
  1390.             y2 = fp_cy + (foot_path[i][1] - center_y) * fp_scale
  1391.             pygame.draw.line(screen, (60,90,70), (int(x1),int(y1)), (int(x2),int(y2)), 1)
  1392.         x1 = fp_cx + (foot_path[-1][0] - center_x) * fp_scale
  1393.         y1 = fp_cy + (foot_path[-1][1] - center_y) * fp_scale
  1394.         x2 = fp_cx + (foot_path[0][0] - center_x) * fp_scale
  1395.         y2 = fp_cy + (foot_path[0][1] - center_y) * fp_scale
  1396.         pygame.draw.line(screen, (60,90,70), (int(x1),int(y1)), (int(x2),int(y2)), 1)
  1397.  
  1398.         if ground_pts:
  1399.             g_sorted = sorted(ground_pts, key=lambda p: p[0])
  1400.             for i in range(1, len(g_sorted)):
  1401.                 gx1 = fp_cx + (g_sorted[i-1][0] - center_x) * fp_scale
  1402.                 gy1 = fp_cy + (g_sorted[i-1][1] - center_y) * fp_scale
  1403.                 gx2 = fp_cx + (g_sorted[i][0] - center_x) * fp_scale
  1404.                 gy2 = fp_cy + (g_sorted[i][1] - center_y) * fp_scale
  1405.                 pygame.draw.line(screen, (100, 200, 100),
  1406.                                  (int(gx1), int(gy1)), (int(gx2), int(gy2)), 3)
  1407.  
  1408.             gxs = [p[0] for p in ground_pts]
  1409.             g_min_x, g_max_x = min(gxs), max(gxs)
  1410.             dim_y = fp_cy + (mx_y - center_y) * fp_scale + 12
  1411.             dim_x1 = fp_cx + (g_min_x - center_x) * fp_scale
  1412.             dim_x2 = fp_cx + (g_max_x - center_x) * fp_scale
  1413.             pygame.draw.line(screen, (80, 160, 80),
  1414.                              (int(dim_x1), int(dim_y)), (int(dim_x2), int(dim_y)), 1)
  1415.             for dx in [dim_x1, dim_x2]:
  1416.                 pygame.draw.line(screen, (80, 160, 80),
  1417.                                  (int(dx), int(dim_y)-3), (int(dx), int(dim_y)+3), 1)
  1418.             dim_mid = (dim_x1 + dim_x2) / 2
  1419.             gnd_label = font_sm.render(f"{ground_in:.1f}\"", True, (100, 200, 100))
  1420.             screen.blit(gnd_label, (int(dim_mid - gnd_label.get_width()/2), int(dim_y)+5))
  1421.  
  1422.         cur = solve_klann(angle, linkage_p)
  1423.         if cur:
  1424.             cx_dot = fp_cx + (cur["F"][0] - center_x) * fp_scale
  1425.             cy_dot = fp_cy + (cur["F"][1] - center_y) * fp_scale
  1426.             pygame.draw.circle(screen, (200, 100, 100), (int(cx_dot), int(cy_dot)), 4)
  1427.  
  1428.         label_y = fp_cy + (mx_y - center_y) * fp_scale + 28
  1429.         screen.blit(font_sm.render("FOOT PATH", True, TEXT_DIM), (fp_cx-30, label_y))
  1430.         screen.blit(font_sm.render(f"{stride_in:.1f}\" x {height_in:.1f}\"",
  1431.                                    True, TEXT_DIM), (fp_cx-25, label_y+14))
  1432.  
  1433.     # --- Actual foot path (amber) below the calculated path ---
  1434.     if len(actual_foot_path) > 2:
  1435.         # Use only the most recent full rotation for the path drawing
  1436.         draw_pts = actual_foot_path[-steps_per_rot:]
  1437.         axs = [pt[0] for pt in draw_pts]
  1438.         ays = [pt[1] for pt in draw_pts]
  1439.         a_mn_x, a_mx_x = min(axs), max(axs)
  1440.         a_mn_y, a_mx_y = min(ays), max(ays)
  1441.         a_w = a_mx_x - a_mn_x
  1442.         a_h = a_mx_y - a_mn_y
  1443.         afp_cx, afp_cy = WIDTH - 120, 280
  1444.         if a_w > 0 and a_h > 0:
  1445.             afp_scale = min(160.0 / a_w, 80.0 / a_h, 1.0)
  1446.         else:
  1447.             afp_scale = 0.5
  1448.         a_center_x = (a_mn_x + a_mx_x) / 2
  1449.         a_center_y = (a_mn_y + a_mx_y) / 2
  1450.  
  1451.         # Draw the path trace
  1452.         n_pts = len(draw_pts)
  1453.         for i in range(1, n_pts):
  1454.             f = i / n_pts
  1455.             col = (int(AMBER[0] * f), int(AMBER[1] * f), int(AMBER[2] * f))
  1456.             ax1 = afp_cx + (draw_pts[i-1][0] - a_center_x) * afp_scale
  1457.             ay1 = afp_cy + (draw_pts[i-1][1] - a_center_y) * afp_scale
  1458.             ax2 = afp_cx + (draw_pts[i][0] - a_center_x) * afp_scale
  1459.             ay2 = afp_cy + (draw_pts[i][1] - a_center_y) * afp_scale
  1460.             pygame.draw.line(screen, col, (int(ax1),int(ay1)), (int(ax2),int(ay2)), 2)
  1461.  
  1462.         # Current position dot
  1463.         last = draw_pts[-1]
  1464.         dx_dot = afp_cx + (last[0] - a_center_x) * afp_scale
  1465.         dy_dot = afp_cy + (last[1] - a_center_y) * afp_scale
  1466.         pygame.draw.circle(screen, AMBER, (int(dx_dot), int(dy_dot)), 4)
  1467.  
  1468.         # --- Average ground contact over last 3 cycles ---
  1469.         n_total = len(actual_foot_path)
  1470.         all_ys = [pt[1] for pt in actual_foot_path]
  1471.         gnd_y = max(all_ys)
  1472.         a_height = gnd_y - min(all_ys)
  1473.         gnd_tol = max(2.0, a_height * 0.03)
  1474.  
  1475.         cycle_contacts = []
  1476.         n_complete = n_total // steps_per_rot
  1477.         for c in range(n_complete):
  1478.             start = n_total - (n_complete - c) * steps_per_rot
  1479.             end = start + steps_per_rot
  1480.             cycle = actual_foot_path[start:end]
  1481.             gnd_pts_c = [pt for pt in cycle if pt[1] >= gnd_y - gnd_tol]
  1482.             if gnd_pts_c:
  1483.                 gxs_c = [pt[0] for pt in gnd_pts_c]
  1484.                 cycle_contacts.append(max(gxs_c) - min(gxs_c))
  1485.             else:
  1486.                 cycle_contacts.append(0.0)
  1487.  
  1488.         avg_contact_px = sum(cycle_contacts) / len(cycle_contacts) if cycle_contacts else 0.0
  1489.         avg_contact_in = avg_contact_px / MM_PER_INCH
  1490.  
  1491.         # Highlight ground contact points on the drawn path (most recent rotation)
  1492.         gnd_draw = [pt for pt in draw_pts if pt[1] >= gnd_y - gnd_tol]
  1493.         if gnd_draw:
  1494.             g_sorted = sorted(gnd_draw, key=lambda pt: pt[0])
  1495.             for i in range(1, len(g_sorted)):
  1496.                 gx1 = afp_cx + (g_sorted[i-1][0] - a_center_x) * afp_scale
  1497.                 gy1 = afp_cy + (g_sorted[i-1][1] - a_center_y) * afp_scale
  1498.                 gx2 = afp_cx + (g_sorted[i][0] - a_center_x) * afp_scale
  1499.                 gy2 = afp_cy + (g_sorted[i][1] - a_center_y) * afp_scale
  1500.                 pygame.draw.line(screen, AMBER,
  1501.                                  (int(gx1), int(gy1)), (int(gx2), int(gy2)), 3)
  1502.  
  1503.             # Dimension line for avg ground contact
  1504.             gxs_draw = [pt[0] for pt in gnd_draw]
  1505.             g_draw_cx = (min(gxs_draw) + max(gxs_draw)) / 2
  1506.             dim_y = afp_cy + (a_mx_y - a_center_y) * afp_scale + 10
  1507.             half_contact_scaled = (avg_contact_px / 2.0) * afp_scale
  1508.             dim_cx = afp_cx + (g_draw_cx - a_center_x) * afp_scale
  1509.             dim_x1 = dim_cx - half_contact_scaled
  1510.             dim_x2 = dim_cx + half_contact_scaled
  1511.             pygame.draw.line(screen, AMBER,
  1512.                              (int(dim_x1), int(dim_y)), (int(dim_x2), int(dim_y)), 1)
  1513.             for dx in [dim_x1, dim_x2]:
  1514.                 pygame.draw.line(screen, AMBER,
  1515.                                  (int(dx), int(dim_y)-3), (int(dx), int(dim_y)+3), 1)
  1516.             dim_mid = (dim_x1 + dim_x2) / 2
  1517.             n_cyc = len(cycle_contacts)
  1518.             contact_label = font_sm.render(
  1519.                 f"{avg_contact_in:.1f}\" avg ({n_cyc} cyc)", True, AMBER)
  1520.             screen.blit(contact_label,
  1521.                         (int(dim_mid - contact_label.get_width()/2), int(dim_y)+5))
  1522.  
  1523.         # Step height from actual data
  1524.         actual_step_height_px = a_mx_y - a_mn_y
  1525.         actual_step_height_in = actual_step_height_px / MM_PER_INCH
  1526.  
  1527.         # Vertical dimension line on the right side of actual path
  1528.         top_scr = afp_cy + (a_mn_y - a_center_y) * afp_scale
  1529.         bot_scr = afp_cy + (a_mx_y - a_center_y) * afp_scale
  1530.         hdim_x = afp_cx + (a_mx_x - a_center_x) * afp_scale + 10
  1531.         pygame.draw.line(screen, AMBER, (int(hdim_x), int(top_scr)), (int(hdim_x), int(bot_scr)), 1)
  1532.         for dy in [int(top_scr), int(bot_scr)]:
  1533.             pygame.draw.line(screen, AMBER, (int(hdim_x)-3, dy), (int(hdim_x)+3, dy), 1)
  1534.         h_label = font_sm.render(f"{actual_step_height_in:.1f}\"", True, AMBER)
  1535.         screen.blit(h_label, (int(hdim_x)+5, int((top_scr + bot_scr)/2) - 6))
  1536.  
  1537.         # Label
  1538.         a_label_y = afp_cy + (a_mx_y - a_center_y) * afp_scale + 28
  1539.         screen.blit(font_sm.render("ACTUAL PATH", True, AMBER), (afp_cx-38, a_label_y))
  1540.  
  1541.     # --- Torque display ---
  1542.     TORQUE_COLOR = (180, 140, 220)
  1543.     TORQUE_DIM   = (120, 95, 155)
  1544.     TORQUE_WARN  = (220, 170, 80)
  1545.     torque_x = WIDTH - 195
  1546.     torque_y = 340
  1547.  
  1548.     # Real-world torque recommendation:
  1549.     # theoretical × assembled_mass_factor × PLA_spacer_friction × impact × starting × safety
  1550.     #               2.0                     1.4                   1.3      1.3        1.2
  1551.     REAL_WORLD_FACTOR = 5.7
  1552.     recommended_nm = max_torque_3cyc * REAL_WORLD_FACTOR
  1553.  
  1554.     # Background panel
  1555.     tp = pygame.Surface((190, 90), pygame.SRCALPHA)
  1556.     tp.fill((18, 22, 32, 200))
  1557.     screen.blit(tp, (torque_x - 5, torque_y - 5))
  1558.  
  1559.     screen.blit(font_sm.render("CRANK TORQUE", True, TORQUE_DIM),
  1560.                 (torque_x, torque_y))
  1561.     screen.blit(font_sm.render(
  1562.         f"instant: {instant_torque:.3f} Nm", True, TORQUE_COLOR),
  1563.         (torque_x, torque_y + 16))
  1564.     screen.blit(font_sm.render(
  1565.         f"max 3cyc: {max_torque_3cyc:.3f} Nm", True, TORQUE_COLOR),
  1566.         (torque_x, torque_y + 32))
  1567.     screen.blit(font_sm.render(
  1568.         f"recommend: {recommended_nm:.2f} Nm", True, TORQUE_WARN),
  1569.         (torque_x, torque_y + 48))
  1570.  
  1571.     # Mini torque bar (visual gauge, scale to recommended)
  1572.     bar_max_nm = max(0.5, recommended_nm * 1.2)
  1573.     bar_w = 170
  1574.     bar_h = 8
  1575.     bar_y = torque_y + 68
  1576.     pygame.draw.rect(screen, SLIDER_BG, (torque_x, bar_y, bar_w, bar_h), border_radius=3)
  1577.     fill_w = min(bar_w, int(bar_w * instant_torque / bar_max_nm))
  1578.     if fill_w > 0:
  1579.         t_ratio = min(1.0, instant_torque / bar_max_nm)
  1580.         bar_r = int(80 + 175 * t_ratio)
  1581.         bar_g = int(200 * (1.0 - t_ratio * 0.7))
  1582.         bar_b = int(80 * (1.0 - t_ratio))
  1583.         pygame.draw.rect(screen, (bar_r, bar_g, bar_b),
  1584.                          (torque_x, bar_y, fill_w, bar_h), border_radius=3)
  1585.     # Recommended line marker on bar
  1586.     rec_x = torque_x + min(bar_w, int(bar_w * max_torque_3cyc * REAL_WORLD_FACTOR / bar_max_nm))
  1587.     pygame.draw.line(screen, TORQUE_WARN, (rec_x, bar_y - 2), (rec_x, bar_y + bar_h + 2), 2)
  1588.  
  1589.     pygame.display.flip()
  1590.  
  1591. pygame.quit()
  1592. sys.exit()
Tags: linkage stl 3dprint Leg
Advertisement
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!