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

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