diff --git a/kinematics/kinematics.py b/kinematics/kinematics.py new file mode 100644 index 0000000..50453eb --- /dev/null +++ b/kinematics/kinematics.py @@ -0,0 +1,246 @@ +""" +kinematics.py — Gordix-style 8-belt suspended CNC router kinematics. + +Geometry: + - 4 corner anchors (top-left, top-right, bottom-left, bottom-right), + each with LEFT and RIGHT belt anchor points offset ±0.05 m from center. + - Sled attachment points offset ±0.035 m from spindle center (X) and + ±0.035 m in Y (front/back for top/bottom corners). + - Sled rides ON the material surface; Z is the vertical plunge depth + of the bit below the sled base. + +Belt length = Euclidean distance between anchor point and sled attachment point. +""" + +from __future__ import annotations + +import math +from collections import namedtuple + +import numpy as np + +# --------------------------------------------------------------------------- +# Named geometry types +# --------------------------------------------------------------------------- + +AnchorSet = namedtuple( + "AnchorSet", + [ + "TL_LEFT", "TL_RIGHT", + "TR_LEFT", "TR_RIGHT", + "BL_LEFT", "BL_RIGHT", + "BR_LEFT", "BR_RIGHT", + ], +) + +SledAttachment = namedtuple( + "SledAttachment", + [ + "SL_TL_LEFT", "SL_TL_RIGHT", + "SL_TR_LEFT", "SL_TR_RIGHT", + "SL_BL_LEFT", "SL_BL_RIGHT", + "SL_BR_LEFT", "SL_BR_RIGHT", + ], +) + +BELT_NAMES = [ + "TL_LEFT", "TL_RIGHT", + "TR_LEFT", "TR_RIGHT", + "BL_LEFT", "BL_RIGHT", + "BR_LEFT", "BR_RIGHT", +] + +# --------------------------------------------------------------------------- +# Fixed anchor geometry (meters, plane Z=0) +# --------------------------------------------------------------------------- + +ANCHORS = AnchorSet( + TL_LEFT=(-0.65, 1.2, 0.0), TL_RIGHT=(-0.55, 1.2, 0.0), + TR_LEFT=(0.55, 1.2, 0.0), TR_RIGHT=(0.65, 1.2, 0.0), + BL_LEFT=(-0.65, -1.2, 0.0), BL_RIGHT=(-0.55, -1.2, 0.0), + BR_LEFT=(0.55, -1.2, 0.0), BR_RIGHT=(0.65, -1.2, 0.0), +) + +# Sled offset from spindle center +SLED_X_OFF = 0.035 # left/right +SLED_Y_OFF = 0.035 # front/back + + +def sled_attachments(x: float, y: float, z: float) -> SledAttachment: + """Return the 8 sled-side belt attachment points for end-effector at (x, y, z).""" + return SledAttachment( + SL_TL_LEFT=(x - SLED_X_OFF, y + SLED_Y_OFF, z), + SL_TL_RIGHT=(x + SLED_X_OFF, y + SLED_Y_OFF, z), + SL_TR_LEFT=(x - SLED_X_OFF, y + SLED_Y_OFF, z), + SL_TR_RIGHT=(x + SLED_X_OFF, y + SLED_Y_OFF, z), + SL_BL_LEFT=(x - SLED_X_OFF, y - SLED_Y_OFF, z), + SL_BL_RIGHT=(x + SLED_X_OFF, y - SLED_Y_OFF, z), + SL_BR_LEFT=(x - SLED_X_OFF, y - SLED_Y_OFF, z), + SL_BR_RIGHT=(x + SLED_X_OFF, y - SLED_Y_OFF, z), + ) + + +_ANCHOR_TUPLE = ( + ANCHORS.TL_LEFT, ANCHORS.TL_RIGHT, + ANCHORS.TR_LEFT, ANCHORS.TR_RIGHT, + ANCHORS.BL_LEFT, ANCHORS.BL_RIGHT, + ANCHORS.BR_LEFT, ANCHORS.BR_RIGHT, +) + + +# --------------------------------------------------------------------------- +# Forward kinematics helper +# --------------------------------------------------------------------------- + +def _compute_lengths_for_pos(x, y, z): + """Return array of 8 belt lengths for end-effector at (x, y, z).""" + sl = sled_attachments(x, y, z) + sled_tuple = ( + sl.SL_TL_LEFT, sl.SL_TL_RIGHT, + sl.SL_TR_LEFT, sl.SL_TR_RIGHT, + sl.SL_BL_LEFT, sl.SL_BL_RIGHT, + sl.SL_BR_LEFT, sl.SL_BR_RIGHT, + ) + return np.array( + [math.dist(a, s) for a, s in zip(_ANCHOR_TUPLE, sled_tuple)] + ) + + +def belt_lengths(x: float, y: float, z: float) -> dict[str, float]: + """Compute all 8 belt lengths for a given end-effector position. + + Returns a dict mapping belt name (e.g. 'TL_LEFT') to length in meters. + """ + lengths = _compute_lengths_for_pos(x, y, z) + return dict(zip(BELT_NAMES, lengths.tolist())) + + +# --------------------------------------------------------------------------- +# Inverse solve (numerical) — given belt lengths, find (x, y, z) +# --------------------------------------------------------------------------- + +def _residual(params, target_lengths): + """Vector of residuals: computed_lengths - target_lengths.""" + x, y, z = params + computed = _compute_lengths_for_pos(x, y, z) + return computed - np.array(target_lengths) + + +def solve_forward( + belt_lengths_dict: dict[str, float], + x0: float = 0.0, + y0: float = 0.0, + z0: float = 0.0, + tol: float = 1e-6, +) -> tuple[float, float, float, dict]: + """Given belt lengths, solve for (x, y, z) using least-squares. + + Returns (x, y, z, info) where info contains solver statistics. + Raises RuntimeError if convergence fails. + """ + from scipy.optimize import least_squares + + target = np.array([ + belt_lengths_dict[n] for n in BELT_NAMES + ]) + + result = least_squares( + _residual, + [x0, y0, z0], + args=(target,), + xtol=tol, + ftol=tol, + max_nfev=2000, + method="trf", # Trust Region Reflective — robust for this problem + ) + + if not result.success: + raise RuntimeError( + f"Forward solve failed: {result.message} (cost={result.cost:.2e})" + ) + + xf, yf, zf = result.x + info = { + "cost": result.cost, + "optimality": result.optimality, + "nfev": result.nfev, + "success": result.success, + } + return xf, yf, zf, info + + +# --------------------------------------------------------------------------- +# Test grid +# --------------------------------------------------------------------------- + +TEST_GRID = [ + ("Center", (0.0, 0.0, 0.0)), + ("Top-Edge", (0.0, 1.2, 0.0)), + ("Bottom-Edge", (0.0, -1.2, 0.0)), + ("Left-Edge", (-0.6, 0.0, 0.0)), + ("Right-Edge", (0.6, 0.0, 0.0)), + ("Top-Left", (-0.6, 1.2, 0.0)), + ("Top-Right", (0.6, 1.2, 0.0)), + ("Bottom-Left", (-0.6, -1.2, 0.0)), + ("Bottom-Right",(0.6, -1.2, 0.0)), +] + + +def _run_test_grid(): + """Run the 9-point test grid and print results.""" + print("=" * 90) + print(" Gordix 8-Belt Kinematics — Test Grid") + print("=" * 90) + print(f"{'Point':<18} {'Belt len range (m)':<24} {'Min':>8} {'Max':>8} " + f"{'Differential':>14} {'Fwd err (mm)':>14} {'Feasible':>10}") + print("-" * 90) + + all_min = float("inf") + all_max = 0.0 + all_ok = True + + for name, (tx, ty, tz) in TEST_GRID: + bl = belt_lengths(tx, ty, tz) + vals = list(bl.values()) + min_l = min(vals) + max_l = max(vals) + diff = max_l - min_l + + # Check geometric feasibility: all belts positive + feasible = all(v > 0.0 for v in vals) + + # Forward solve to verify inverse consistency + fwd_err = float("nan") + try: + xf, yf, zf, info = solve_forward(bl, x0=tx, y0=ty, z0=tz) + fwd_err = math.dist((tx, ty, tz), (xf, yf, zf)) * 1000.0 # mm + except RuntimeError as e: + feasible = False + + ok = feasible and (not math.isnan(fwd_err) and fwd_err <= 1.0) + + print( + f" {name:<16} {min_l:.6f} – {max_l:.6f} " + f"{min_l:>8.4f} {max_l:>8.4f} {diff:>8.4f} " + f"{fwd_err:>10.4f} {'✓' if ok else '✗':>8}" + ) + + all_min = min(all_min, min_l) + all_max = max(all_max, max_l) + if not ok: + all_ok = False + + print("-" * 90) + print(f" Global min belt length: {all_min:.6f} m") + print(f" Global max belt length: {all_max:.6f} m") + print(f" Overall feasible: {'YES ✓' if all_ok else 'FAIL ✗'}") + print("=" * 90) + return all_ok + + +# --------------------------------------------------------------------------- +# Command-line entry point +# --------------------------------------------------------------------------- + +if __name__ == "__main__": + _run_test_grid() diff --git a/kinematics/simulate_grid.py b/kinematics/simulate_grid.py new file mode 100644 index 0000000..63b861d --- /dev/null +++ b/kinematics/simulate_grid.py @@ -0,0 +1,168 @@ +""" +simulate_grid.py — Sweep a 10×10 grid across the workspace and analyze +tension differential, belt lengths, and worst-case positions. + +Outputs: + - workspace_heatmap.png (matplotlib heatmap of tension differential) + - workspace_heatmap.csv (fallback if no matplotlib, also written as data log) +""" + +from __future__ import annotations + +import csv +import math +import os +import sys + +import numpy as np + +from kinematics import ( + belt_lengths, + solve_forward, + BELT_NAMES, + TEST_GRID, +) +from tension_analysis import analyze_tension, resting_lengths + +# Output directory +OUTPUT_DIR = os.path.dirname(os.path.abspath(__file__)) + +# Workspace bounds +X_MIN, X_MAX = -0.6, 0.6 +Y_MIN, Y_MAX = -1.2, 1.2 + + +def sweep_grid(nx: int = 10, ny: int = 10, + z: float = 0.0) -> tuple[np.ndarray, np.ndarray, np.ndarray, tuple]: + """Sweep an nx × ny grid across the workspace at height z. + + Returns: + xs, ys: 1D arrays of X and Y grid lines + diff_map: (ny, nx) array of max-min tension differential at each point + worst: ((x, y), max_diff) — the point with greatest tension differential + """ + xs = np.linspace(X_MIN, X_MAX, nx) + ys = np.linspace(Y_MIN, Y_MAX, ny) + + diff_map = np.zeros((ny, nx)) + worst_diff = 0.0 + worst_xy = (0.0, 0.0) + + rest = resting_lengths(0.0, 0.0, 0.0) + + for i, x in enumerate(xs): + for j, y in enumerate(ys): + result = analyze_tension(x, y, z, rest_lengths=rest) + tmin = np.min(result.tension_multipliers) + tmax = np.max(result.tension_multipliers) + diff = tmax - tmin + diff_map[j, i] = diff + if diff > worst_diff: + worst_diff = diff + worst_xy = (x, y) + + return xs, ys, diff_map, (worst_xy, worst_diff) + + +def write_csv(xs: np.ndarray, ys: np.ndarray, + diff_map: np.ndarray, path: str) -> None: + """Write the grid data as a CSV file.""" + with open(path, "w", newline="") as f: + writer = csv.writer(f) + # Header: first cell empty, then X coordinates + header = [""] + [f"{x:.6f}" for x in xs] + writer.writerow(header) + for j, y in enumerate(ys): + row = [f"{y:.6f}"] + [f"{diff_map[j, i]:.6f}" for i in range(len(xs))] + writer.writerow(row) + print(f" Wrote CSV: {path}") + + +def plot_heatmap(xs: np.ndarray, ys: np.ndarray, + diff_map: np.ndarray, worst_xy: tuple, + worst_diff: float, + path: str) -> bool: + """Generate and save a heatmap using matplotlib. + + Returns True on success, False if matplotlib is unavailable. + """ + try: + import matplotlib.pyplot as plt + except ImportError: + return False + + fig, ax = plt.subplots(figsize=(10, 8)) + + X, Y = np.meshgrid(xs, ys) + levels = 50 + cf = ax.contourf(X, Y, diff_map, levels=levels, cmap="plasma") + cbar = fig.colorbar(cf, ax=ax, label="Tension Differential (multiplier range)") + + # Mark worst point + wx, wy = worst_xy + ax.plot(wx, wy, marker="*", color="white", markersize=14, + markeredgecolor="black", markeredgewidth=1.0) + ax.annotate(f"Worst: ({wx:.3f}, {wy:.3f})\nDiff = {worst_diff:.3f}", + xy=(wx, wy), xytext=(wx + 0.12, wy + 0.08), + color="white", fontsize=9, + arrowprops=dict(arrowstyle="->", color="white", lw=1.2), + bbox=dict(boxstyle="round,pad=0.3", facecolor="black", + edgecolor="white", alpha=0.7)) + + # Mark the 9 test points + for name, (tx, ty, tz) in TEST_GRID: + ax.plot(tx, ty, marker="o", color="cyan", markersize=4, alpha=0.8) + + ax.set_xlabel("X (m)") + ax.set_ylabel("Y (m)") + ax.set_title("Gordix 8-Belt — Tension Differential Across Workspace\n" + "(10×10 grid, Z=0)") + ax.set_aspect("equal") + ax.grid(True, alpha=0.3) + + fig.tight_layout() + fig.savefig(path, dpi=150) + plt.close(fig) + print(f" Saved heatmap: {path}") + return True + + +def _run_sweep(): + print("=" * 70) + print(" Grid Sweep — Gordix 8-Belt Workspace Analysis") + print("=" * 70) + + xs, ys, diff_map, (worst_xy, worst_diff) = sweep_grid(10, 10, 0.0) + + print(f"\n Grid: 10 × 10 = 100 points") + print(f" Workspace: X=[{X_MIN:.2f}, {X_MAX:.2f}] Y=[{Y_MIN:.2f}, {Y_MAX:.2f}]") + print(f"\n Worst-case tension differential:") + print(f" Point: ({worst_xy[0]:.4f}, {worst_xy[1]:.4f}) m") + print(f" Differential: {worst_diff:.4f} (tension multiplier range)") + + # Also report raw belt length range + rest = resting_lengths(0.0, 0.0, 0.0) + bl = belt_lengths(worst_xy[0], worst_xy[1], 0.0) + vals = list(bl.values()) + print(f" Belt lengths: {min(vals):.6f} – {max(vals):.6f} m") + print(f" ΔL from rest:") + for name in BELT_NAMES: + delta = (bl[name] - rest[name]) * 1000 + print(f" {name:<12}: {delta:+8.4f} mm") + + # Save CSV always + csv_path = os.path.join(OUTPUT_DIR, "workspace_heatmap.csv") + write_csv(xs, ys, diff_map, csv_path) + + # Save PNG if possible + png_path = os.path.join(OUTPUT_DIR, "workspace_heatmap.png") + ok = plot_heatmap(xs, ys, diff_map, worst_xy, worst_diff, png_path) + if not ok: + print(" [matplotlib not available — skipped PNG, CSV saved]") + + print() + return worst_xy, worst_diff + + +if __name__ == "__main__": + _run_sweep() diff --git a/kinematics/tension_analysis.py b/kinematics/tension_analysis.py new file mode 100644 index 0000000..b729cb4 --- /dev/null +++ b/kinematics/tension_analysis.py @@ -0,0 +1,155 @@ +""" +tension_analysis.py — Motor spool rotation and tension estimation +for a Gordix-style 8-belt suspended CNC router. + +Given belt lengths from kinematics.py, estimates: + - Motor spool rotation (radians) for each belt + - Tension multiplier per belt relative to the average + +Assumptions: + - All belts have identical linear stiffness (EA constant). + - Tension = EA * strain, where strain ≈ (L - L_rest) / L_rest. + - All belts share the same resting length (nominal length at center position). + - Spool radius is configurable (default 0.015 m). +""" + +from __future__ import annotations + +import math +from collections import namedtuple + +import numpy as np + +from kinematics import belt_lengths, BELT_NAMES + +TensionResult = namedtuple( + "TensionResult", + ["belt_names", "lengths", "spool_rotations", "tension_multipliers"], +) + + +def resting_lengths(center_x: float = 0.0, + center_y: float = 0.0, + center_z: float = 0.0) -> dict[str, float]: + """Compute the nominal (resting) belt lengths at a given position. + + This is the length each belt would have when the sled is at the + home/center position. All tensions are referenced to these lengths. + """ + return belt_lengths(center_x, center_y, center_z) + + +def analyze_tension(x: float, + y: float, + z: float, + spool_radius: float = 0.015, + rest_lengths: dict[str, float] | None = None, + ) -> TensionResult: + """Analyze tension and spool rotation at a given end-effector position. + + Args: + x, y, z: End-effector position in meters. + spool_radius: Motor spool radius (default 0.015 m). + rest_lengths: Resting belt lengths (from resting_lengths()). If None, + computed at (0, 0, 0). + + Returns: + TensionResult with fields: + - belt_names: list of 8 belt name strings + - lengths: np.array of current belt lengths + - spool_rotations: np.array of spool rotations in radians + - tension_multipliers: np.array of relative tension (1.0 = average) + """ + if rest_lengths is None: + rest_lengths = resting_lengths(0.0, 0.0, 0.0) + + current_lengths = belt_lengths(x, y, z) + + names = list(BELT_NAMES) + L_curr = np.array([current_lengths[n] for n in names]) + L_rest = np.array([rest_lengths[n] for n in names]) + + # Spool rotation: how much belt must be paid out/taken up + delta = L_curr - L_rest + spool_rot = delta / spool_radius + + # Tension estimate: T = EA * (L - L_rest) / L_rest + # We only care about relative tension, so EA cancels. + strain = delta / L_rest + # Avoid division by zero — clamp minimum strain for multiplier calc + min_strain = np.min(strain) + if min_strain < 0: + # Some belts could be under zero strain (shorter than rest) + # We report as-is; negative = slack + pass + + # Tension multiplier = strain / mean(|strain|) + mean_abs_strain = np.mean(np.abs(strain)) + if mean_abs_strain < 1e-12: + tension_mult = np.ones_like(strain) + else: + tension_mult = strain / mean_abs_strain + + return TensionResult( + belt_names=list(names), + lengths=L_curr, + spool_rotations=spool_rot, + tension_multipliers=tension_mult, + ) + + +def print_analysis(x: float, y: float, z: float, + spool_radius: float = 0.015) -> None: + """Pretty-print tension analysis for a single position.""" + result = analyze_tension(x, y, z, spool_radius) + + print(f"\n Tension Analysis @ ({x:.3f}, {y:.3f}, {z:.3f}) m") + print(f" Spool radius: {spool_radius:.3f} m") + print(f" {'Belt':<12} {'Length (m)':<12} {'ΔL (mm)':<12} " + f"{'Spool (rad)':<12} {'Tension mult':<12}") + print(" " + "-" * 60) + + L_rest = resting_lengths(0.0, 0.0, 0.0) + + for i, name in enumerate(result.belt_names): + delta_mm = (result.lengths[i] - L_rest[name]) * 1000.0 + print(f" {name:<12} {result.lengths[i]:<12.6f} {delta_mm:<12.4f} " + f"{result.spool_rotations[i]:<12.3f} {result.tension_multipliers[i]:<12.4f}") + + print(f"\n Max spool rotation: {np.max(np.abs(result.spool_rotations)):.3f} rad") + print(f" Max tension multiplier: {np.max(result.tension_multipliers):.4f}") + print(f" Min tension multiplier: {np.min(result.tension_multipliers):.4f}") + print() + + +if __name__ == "__main__": + print("=" * 70) + print(" Tension Analysis — Gordix 8-Belt Kinematics") + print("=" * 70) + + # Analyze the corner positions (worst-case normally) + test_positions = [ + ("Center", 0.0, 0.0, 0.0), + ("Top-Left", -0.6, 1.2, 0.0), + ("Top-Right", 0.6, 1.2, 0.0), + ("Bottom-Left",-0.6, -1.2, 0.0), + ("Bottom-Right",0.6, -1.2, 0.0), + ] + + for label, x, y, z in test_positions: + print(f"\n--- {label} ---") + print_analysis(x, y, z) + + # Summary across the 5 positions + print("=" * 70) + print(" Summary: Spool & Tension Range") + print("=" * 70) + print(f" {'Position':<16} {'Max |spool| (rad)':<20} {'Max tension mult':<18} " + f"{'Min tension mult':<18}") + print(" " + "-" * 72) + for label, x, y, z in test_positions: + r = analyze_tension(x, y, z) + print(f" {label:<16} {np.max(np.abs(r.spool_rotations)):<20.3f} " + f"{np.max(r.tension_multipliers):<18.4f} " + f"{np.min(r.tension_multipliers):<18.4f}") + print() diff --git a/kinematics/workspace_heatmap.csv b/kinematics/workspace_heatmap.csv new file mode 100644 index 0000000..37d4e0e --- /dev/null +++ b/kinematics/workspace_heatmap.csv @@ -0,0 +1,11 @@ +,-0.600000,-0.466667,-0.333333,-0.200000,-0.066667,0.066667,0.200000,0.333333,0.466667,0.600000 +-1.200000,2.774871,2.637202,2.467817,2.292166,2.112956,2.112956,2.292166,2.467817,2.637202,2.774871 +-0.933333,2.863433,2.709214,2.529806,2.333065,2.127500,2.127500,2.333065,2.529806,2.709214,2.863433 +-0.666667,3.052784,2.842955,2.624982,2.391192,2.146682,2.146682,2.391192,2.624982,2.842955,3.052784 +-0.400000,3.080165,3.240384,2.913572,2.563823,2.203642,2.203642,2.563823,2.913572,3.240384,3.080165 +-0.133333,2.911071,3.147304,3.405083,3.553844,2.532327,2.532327,3.553844,3.405083,3.147304,2.911071 +0.133333,2.911071,3.147304,3.405083,3.553844,2.532327,2.532327,3.553844,3.405083,3.147304,2.911071 +0.400000,3.080165,3.240384,2.913572,2.563823,2.203642,2.203642,2.563823,2.913572,3.240384,3.080165 +0.666667,3.052784,2.842955,2.624982,2.391192,2.146682,2.146682,2.391192,2.624982,2.842955,3.052784 +0.933333,2.863433,2.709214,2.529806,2.333065,2.127500,2.127500,2.333065,2.529806,2.709214,2.863433 +1.200000,2.774871,2.637202,2.467817,2.292166,2.112956,2.112956,2.292166,2.467817,2.637202,2.774871 diff --git a/kinematics/workspace_heatmap.png b/kinematics/workspace_heatmap.png new file mode 100644 index 0000000..f01276c Binary files /dev/null and b/kinematics/workspace_heatmap.png differ