feat: 8-belt kinematic simulation model + tension/workspace analysis
This commit is contained in:
246
kinematics/kinematics.py
Normal file
246
kinematics/kinematics.py
Normal file
@@ -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()
|
||||
Reference in New Issue
Block a user