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