feat: 8-belt kinematic simulation model + tension/workspace analysis

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()

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()

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()

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