247 lines
7.5 KiB
Python
247 lines
7.5 KiB
Python
"""
|
||
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()
|