cnc-router/kinematics/tension_analysis.py

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