toolwake 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- toolwake/__init__.py +51 -0
- toolwake/cli.py +75 -0
- toolwake/deposit.py +150 -0
- toolwake/geometry.py +179 -0
- toolwake/profile.py +181 -0
- toolwake/render.py +173 -0
- toolwake/serve.py +232 -0
- toolwake/simulate.py +142 -0
- toolwake/tool.py +124 -0
- toolwake/toolpath.py +228 -0
- toolwake/viewer.py +430 -0
- toolwake-0.1.0.dist-info/METADATA +223 -0
- toolwake-0.1.0.dist-info/RECORD +17 -0
- toolwake-0.1.0.dist-info/WHEEL +5 -0
- toolwake-0.1.0.dist-info/entry_points.txt +2 -0
- toolwake-0.1.0.dist-info/licenses/LICENSE +21 -0
- toolwake-0.1.0.dist-info/top_level.txt +1 -0
toolwake/__init__.py
ADDED
|
@@ -0,0 +1,51 @@
|
|
|
1
|
+
"""toolwake — simulate a depositing tool, accumulate its wake, check against it.
|
|
2
|
+
|
|
3
|
+
from toolwake import Toolpath, Needle, simulate, animate
|
|
4
|
+
|
|
5
|
+
path = Toolpath.helix(radius=0.02, pitch=0.004, turns=6)
|
|
6
|
+
res = simulate(path, Needle(inner_d=90e-6, housing=(0.130, 0.075, 0.072)))
|
|
7
|
+
print(res.report())
|
|
8
|
+
animate(res, "wake.mp4")
|
|
9
|
+
|
|
10
|
+
The wake the video draws and the wake the clearance numbers are measured
|
|
11
|
+
against are the same object, so the picture cannot disagree with the report.
|
|
12
|
+
"""
|
|
13
|
+
from .deposit import Deposit
|
|
14
|
+
from .geometry import (Box, Capsule, rotation_from_rotvec,
|
|
15
|
+
rotvec_from_axis)
|
|
16
|
+
from .simulate import Result, simulate
|
|
17
|
+
from .viewer import to_html_str
|
|
18
|
+
from .profile import Section, ToolProfile, blunt_cannula, luer_taper_tip
|
|
19
|
+
from .tool import Needle, ToolPose
|
|
20
|
+
from .toolpath import PRINT, TRAVEL, Toolpath
|
|
21
|
+
|
|
22
|
+
__version__ = "0.1.0"
|
|
23
|
+
|
|
24
|
+
__all__ = [
|
|
25
|
+
"Toolpath", "PRINT", "TRAVEL",
|
|
26
|
+
"Needle", "ToolPose",
|
|
27
|
+
"Section", "ToolProfile", "blunt_cannula", "luer_taper_tip",
|
|
28
|
+
"Deposit",
|
|
29
|
+
"Capsule", "Box", "rotation_from_rotvec", "rotvec_from_axis",
|
|
30
|
+
"simulate", "Result",
|
|
31
|
+
"animate", "to_html", "to_html_str", "ToolwakeSession",
|
|
32
|
+
"__version__",
|
|
33
|
+
]
|
|
34
|
+
|
|
35
|
+
|
|
36
|
+
def animate(*args, **kwargs):
|
|
37
|
+
"""Render a Result to video. Imported lazily so matplotlib stays optional."""
|
|
38
|
+
from .render import animate as _animate
|
|
39
|
+
return _animate(*args, **kwargs)
|
|
40
|
+
|
|
41
|
+
|
|
42
|
+
def ToolwakeSession(*args, **kwargs):
|
|
43
|
+
"""A re-checkable simulation served as HTML, for embedding in another GUI."""
|
|
44
|
+
from .serve import ToolwakeSession as _S
|
|
45
|
+
return _S(*args, **kwargs)
|
|
46
|
+
|
|
47
|
+
|
|
48
|
+
def to_html(*args, **kwargs):
|
|
49
|
+
"""Write a standalone interactive viewer. No dependencies beyond numpy."""
|
|
50
|
+
from .viewer import to_html as _to_html
|
|
51
|
+
return _to_html(*args, **kwargs)
|
toolwake/cli.py
ADDED
|
@@ -0,0 +1,75 @@
|
|
|
1
|
+
"""Command line: toolwake render <path> -o out.mp4"""
|
|
2
|
+
from __future__ import annotations
|
|
3
|
+
|
|
4
|
+
import argparse
|
|
5
|
+
import json
|
|
6
|
+
import sys
|
|
7
|
+
|
|
8
|
+
from .simulate import simulate
|
|
9
|
+
from .tool import Needle
|
|
10
|
+
from .toolpath import Toolpath
|
|
11
|
+
|
|
12
|
+
|
|
13
|
+
def _bar(label):
|
|
14
|
+
def show(i, n):
|
|
15
|
+
pct = 100.0 * i / n
|
|
16
|
+
sys.stderr.write(f"\r {label}: {pct:5.1f}% ({i}/{n})")
|
|
17
|
+
if i >= n:
|
|
18
|
+
sys.stderr.write("\n")
|
|
19
|
+
sys.stderr.flush()
|
|
20
|
+
return show
|
|
21
|
+
|
|
22
|
+
|
|
23
|
+
def main(argv=None) -> int:
|
|
24
|
+
p = argparse.ArgumentParser(
|
|
25
|
+
prog="toolwake",
|
|
26
|
+
description="Simulate a depositing tool and the wake it leaves behind.")
|
|
27
|
+
p.add_argument("source", help="a .gcode file, or 'helix' for the built-in demo")
|
|
28
|
+
p.add_argument("-o", "--out", default="wake.mp4",
|
|
29
|
+
help="video output (.mp4 needs ffmpeg, .gif does not)")
|
|
30
|
+
p.add_argument("--inner-d", type=float, default=90e-6, help="needle ID, m")
|
|
31
|
+
p.add_argument("--outer-d", type=float, default=None, help="needle OD, m")
|
|
32
|
+
p.add_argument("--length", type=float, default=12.7e-3, help="needle length, m")
|
|
33
|
+
p.add_argument("--housing", type=float, nargs=3, default=None,
|
|
34
|
+
metavar=("X", "Y", "Z"), help="end-effector extents, m")
|
|
35
|
+
p.add_argument("--lag", type=int, default=8,
|
|
36
|
+
help="rows of immunity behind the nozzle")
|
|
37
|
+
p.add_argument("--threshold", type=float, default=5e-4,
|
|
38
|
+
help="clearance below which a row is 'close', m")
|
|
39
|
+
p.add_argument("--max-frames", type=int, default=600)
|
|
40
|
+
p.add_argument("--fps", type=int, default=30)
|
|
41
|
+
p.add_argument("--report", default=None, help="write the JSON report here")
|
|
42
|
+
p.add_argument("--html", default=None,
|
|
43
|
+
help="write a standalone interactive viewer here")
|
|
44
|
+
p.add_argument("--no-video", action="store_true", help="skip the video")
|
|
45
|
+
a = p.parse_args(argv)
|
|
46
|
+
|
|
47
|
+
path = (Toolpath.helix() if a.source == "helix"
|
|
48
|
+
else Toolpath.from_gcode(a.source))
|
|
49
|
+
print(f" {path!r}", file=sys.stderr)
|
|
50
|
+
|
|
51
|
+
needle = Needle(inner_d=a.inner_d, outer_d=a.outer_d, length=a.length,
|
|
52
|
+
housing=a.housing)
|
|
53
|
+
res = simulate(path, needle, lag=a.lag, threshold=a.threshold,
|
|
54
|
+
progress=_bar("sweep"))
|
|
55
|
+
rep = res.report()
|
|
56
|
+
print(json.dumps(rep, indent=2))
|
|
57
|
+
if a.report:
|
|
58
|
+
with open(a.report, "w") as fh:
|
|
59
|
+
json.dump(rep, fh, indent=2)
|
|
60
|
+
|
|
61
|
+
if a.html:
|
|
62
|
+
from .viewer import to_html
|
|
63
|
+
print(f" wrote {to_html(res, a.html)}", file=sys.stderr)
|
|
64
|
+
|
|
65
|
+
if not a.no_video:
|
|
66
|
+
from .render import animate
|
|
67
|
+
out = animate(res, a.out, fps=a.fps, max_frames=a.max_frames,
|
|
68
|
+
progress=_bar("render"))
|
|
69
|
+
print(f" wrote {out}", file=sys.stderr)
|
|
70
|
+
|
|
71
|
+
return 2 if rep["status"] == "collision" else 0
|
|
72
|
+
|
|
73
|
+
|
|
74
|
+
if __name__ == "__main__":
|
|
75
|
+
raise SystemExit(main())
|
toolwake/deposit.py
ADDED
|
@@ -0,0 +1,150 @@
|
|
|
1
|
+
"""The wake: material already laid down, and how to ask what is near it.
|
|
2
|
+
|
|
3
|
+
Two-phase by design.
|
|
4
|
+
|
|
5
|
+
Broad phase — a uniform spatial hash (a voxel grid holding segment indices).
|
|
6
|
+
O(1) lookup, answers "which beads could possibly be near here".
|
|
7
|
+
Narrow phase — exact capsule distance to those candidate beads only.
|
|
8
|
+
|
|
9
|
+
A plain occupancy grid would give a binary answer quantised to the voxel size;
|
|
10
|
+
keeping the segments and measuring against them means the clearance number in
|
|
11
|
+
the report is a real distance, not a voxel count. The grid exists purely to
|
|
12
|
+
avoid testing every bead against every tool pose.
|
|
13
|
+
|
|
14
|
+
Time ordering is the other reason this class exists. Material can only obstruct
|
|
15
|
+
the tool if it was deposited EARLIER, and not so recently that it is still
|
|
16
|
+
under the nozzle. `add` appends in path order and `query` takes the frame index,
|
|
17
|
+
so the trailing-window rule lives in one place instead of at every call site.
|
|
18
|
+
"""
|
|
19
|
+
from __future__ import annotations
|
|
20
|
+
|
|
21
|
+
import numpy as np
|
|
22
|
+
|
|
23
|
+
__all__ = ["Deposit"]
|
|
24
|
+
|
|
25
|
+
|
|
26
|
+
class Deposit:
|
|
27
|
+
"""Accumulates deposited bead segments and answers clearance queries.
|
|
28
|
+
|
|
29
|
+
Args:
|
|
30
|
+
bead_radius: radius of the laid bead, metres. For a well-tuned print
|
|
31
|
+
this is about half the needle inner diameter.
|
|
32
|
+
cell: spatial-hash cell size, metres. This wants to be on the order of
|
|
33
|
+
the QUERY radius, not the bead. Sized to the bead (45 um) against a
|
|
34
|
+
20 mm search, a single lookup walks 41^3 = 69k cells and the sweep
|
|
35
|
+
crawls; at 5 mm it walks 9^3 = 729. Callers that know their search
|
|
36
|
+
radius should pass `search / 4`.
|
|
37
|
+
"""
|
|
38
|
+
|
|
39
|
+
def __init__(self, bead_radius: float, cell: float | None = None):
|
|
40
|
+
if bead_radius < 0:
|
|
41
|
+
raise ValueError("bead_radius must be >= 0")
|
|
42
|
+
self.bead_radius = float(bead_radius)
|
|
43
|
+
self.cell = float(cell) if cell else max(8.0 * self.bead_radius, 5e-3)
|
|
44
|
+
self._a: list[np.ndarray] = [] # segment starts
|
|
45
|
+
self._b: list[np.ndarray] = [] # segment ends
|
|
46
|
+
self._t: list[int] = [] # frame index each was laid at
|
|
47
|
+
self._grid: dict[tuple, list[int]] = {}
|
|
48
|
+
|
|
49
|
+
# ---------------------------------------------------------------- build
|
|
50
|
+
def __len__(self) -> int:
|
|
51
|
+
return len(self._a)
|
|
52
|
+
|
|
53
|
+
def _cells_for(self, a: np.ndarray, b: np.ndarray):
|
|
54
|
+
"""Every grid cell the segment's padded AABB touches."""
|
|
55
|
+
lo = np.minimum(a, b) - self.bead_radius
|
|
56
|
+
hi = np.maximum(a, b) + self.bead_radius
|
|
57
|
+
lo_i = np.floor(lo / self.cell).astype(int)
|
|
58
|
+
hi_i = np.floor(hi / self.cell).astype(int)
|
|
59
|
+
for i in range(lo_i[0], hi_i[0] + 1):
|
|
60
|
+
for j in range(lo_i[1], hi_i[1] + 1):
|
|
61
|
+
for k in range(lo_i[2], hi_i[2] + 1):
|
|
62
|
+
yield (i, j, k)
|
|
63
|
+
|
|
64
|
+
def add(self, a, b, frame: int) -> None:
|
|
65
|
+
"""Record one deposited segment, laid at `frame`."""
|
|
66
|
+
a = np.asarray(a, dtype=float)
|
|
67
|
+
b = np.asarray(b, dtype=float)
|
|
68
|
+
idx = len(self._a)
|
|
69
|
+
self._a.append(a)
|
|
70
|
+
self._b.append(b)
|
|
71
|
+
self._t.append(int(frame))
|
|
72
|
+
for c in self._cells_for(a, b):
|
|
73
|
+
self._grid.setdefault(c, []).append(idx)
|
|
74
|
+
|
|
75
|
+
# ---------------------------------------------------------------- query
|
|
76
|
+
def _candidates(self, lo: np.ndarray, hi: np.ndarray) -> np.ndarray:
|
|
77
|
+
lo_i = np.floor(lo / self.cell).astype(int)
|
|
78
|
+
hi_i = np.floor(hi / self.cell).astype(int)
|
|
79
|
+
out: set[int] = set()
|
|
80
|
+
for i in range(lo_i[0], hi_i[0] + 1):
|
|
81
|
+
for j in range(lo_i[1], hi_i[1] + 1):
|
|
82
|
+
for k in range(lo_i[2], hi_i[2] + 1):
|
|
83
|
+
hit = self._grid.get((i, j, k))
|
|
84
|
+
if hit:
|
|
85
|
+
out.update(hit)
|
|
86
|
+
return np.fromiter(out, dtype=int, count=len(out))
|
|
87
|
+
|
|
88
|
+
def clearance(self, shape, frame: int, lag: int = 0,
|
|
89
|
+
search: float = 0.02) -> tuple[float, int]:
|
|
90
|
+
"""Smallest gap between `shape` and material laid before `frame - lag`.
|
|
91
|
+
|
|
92
|
+
Args:
|
|
93
|
+
shape: anything with `.distance(points)` and `.bounds(pad)` —
|
|
94
|
+
a Capsule or a Box from `toolwake.geometry`.
|
|
95
|
+
frame: the current frame index.
|
|
96
|
+
lag: how many frames back to start counting material as an
|
|
97
|
+
obstacle. Without this the tool always collides with the bead
|
|
98
|
+
it is extruding right now.
|
|
99
|
+
search: broad-phase radius, metres. Beads further away than this
|
|
100
|
+
are not measured at all; the returned distance is clipped to it.
|
|
101
|
+
|
|
102
|
+
Returns:
|
|
103
|
+
(clearance, segment_index). `inf` and -1 when nothing is in range.
|
|
104
|
+
"""
|
|
105
|
+
if not self._a:
|
|
106
|
+
return float("inf"), -1
|
|
107
|
+
|
|
108
|
+
lo, hi = shape.bounds(pad=search)
|
|
109
|
+
cand = self._candidates(lo, hi)
|
|
110
|
+
if cand.size == 0:
|
|
111
|
+
return float("inf"), -1
|
|
112
|
+
|
|
113
|
+
t = np.asarray(self._t)[cand]
|
|
114
|
+
cand = cand[t < frame - lag] # earlier material only
|
|
115
|
+
if cand.size == 0:
|
|
116
|
+
return float("inf"), -1
|
|
117
|
+
|
|
118
|
+
A = np.asarray(self._a)[cand]
|
|
119
|
+
B = np.asarray(self._b)[cand]
|
|
120
|
+
# Sample each candidate bead along its axis; with beads this short
|
|
121
|
+
# relative to the tool, endpoints plus midpoint bound the true
|
|
122
|
+
# segment-to-segment distance closely and stay fully vectorised.
|
|
123
|
+
pts = np.vstack([A, B, 0.5 * (A + B)])
|
|
124
|
+
d = shape.distance(pts) - self.bead_radius
|
|
125
|
+
n = len(cand)
|
|
126
|
+
d = np.min(d.reshape(3, n), axis=0)
|
|
127
|
+
j = int(np.argmin(d))
|
|
128
|
+
return float(d[j]), int(cand[j])
|
|
129
|
+
|
|
130
|
+
# ---------------------------------------------------------------- views
|
|
131
|
+
def segments(self, upto: int | None = None) -> np.ndarray:
|
|
132
|
+
"""(N, 2, 3) array of laid segments, for drawing. `upto` filters by frame."""
|
|
133
|
+
if not self._a:
|
|
134
|
+
return np.zeros((0, 2, 3))
|
|
135
|
+
A = np.asarray(self._a)
|
|
136
|
+
B = np.asarray(self._b)
|
|
137
|
+
if upto is not None:
|
|
138
|
+
keep = np.asarray(self._t) <= upto
|
|
139
|
+
A, B = A[keep], B[keep]
|
|
140
|
+
return np.stack([A, B], axis=1)
|
|
141
|
+
|
|
142
|
+
def bounds(self):
|
|
143
|
+
if not self._a:
|
|
144
|
+
return None
|
|
145
|
+
pts = np.vstack([np.asarray(self._a), np.asarray(self._b)])
|
|
146
|
+
return pts.min(axis=0) - self.bead_radius, pts.max(axis=0) + self.bead_radius
|
|
147
|
+
|
|
148
|
+
def __repr__(self):
|
|
149
|
+
return (f"Deposit({len(self)} segments, bead_r={self.bead_radius:.4g}, "
|
|
150
|
+
f"cell={self.cell:.4g}, {len(self._grid)} cells)")
|
toolwake/geometry.py
ADDED
|
@@ -0,0 +1,179 @@
|
|
|
1
|
+
"""Exact distance primitives for the tool side of the check.
|
|
2
|
+
|
|
3
|
+
The tool is modelled with primitives rather than meshes on purpose. On the rig
|
|
4
|
+
this was written for, the end-effector's collision geometry in the URDF is
|
|
5
|
+
already a box and the needle is a thin cylinder, so a capsule and an oriented
|
|
6
|
+
box reproduce the real collision volume exactly — nothing is approximated away,
|
|
7
|
+
and there is no mesh library to depend on.
|
|
8
|
+
|
|
9
|
+
All functions are vectorised over a set of query points, because the caller
|
|
10
|
+
tests one tool pose against many deposited points at once.
|
|
11
|
+
"""
|
|
12
|
+
from __future__ import annotations
|
|
13
|
+
|
|
14
|
+
import numpy as np
|
|
15
|
+
|
|
16
|
+
__all__ = ["Capsule", "Box", "rotation_from_rotvec", "rotvec_from_axis",
|
|
17
|
+
"rpy_from_rotation"]
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
def rotation_from_rotvec(rotvec) -> np.ndarray:
|
|
21
|
+
"""Rotation matrix from a rotation vector (axis * angle), via Rodrigues.
|
|
22
|
+
|
|
23
|
+
Matches the convention UR controllers use for pose orientation, so a pose
|
|
24
|
+
row read straight off the robot can be passed in unchanged.
|
|
25
|
+
"""
|
|
26
|
+
r = np.asarray(rotvec, dtype=float)
|
|
27
|
+
theta = float(np.linalg.norm(r))
|
|
28
|
+
if theta < 1e-12:
|
|
29
|
+
return np.eye(3)
|
|
30
|
+
k = r / theta
|
|
31
|
+
K = np.array([[0.0, -k[2], k[1]],
|
|
32
|
+
[k[2], 0.0, -k[0]],
|
|
33
|
+
[-k[1], k[0], 0.0]])
|
|
34
|
+
return np.eye(3) + np.sin(theta) * K + (1.0 - np.cos(theta)) * (K @ K)
|
|
35
|
+
|
|
36
|
+
|
|
37
|
+
def rotvec_from_axis(u, reference=(0.0, 0.0, 1.0)) -> np.ndarray:
|
|
38
|
+
"""Rotation vector taking `reference` onto `u` by the shortest arc.
|
|
39
|
+
|
|
40
|
+
The inverse of what `Needle.at` needs: give it the direction the tool
|
|
41
|
+
should point (tip-to-body, so "up the tool") and it returns the rotation
|
|
42
|
+
vector to store on the toolpath.
|
|
43
|
+
|
|
44
|
+
The shortest arc leaves the roll about the tool axis unconstrained, which
|
|
45
|
+
is correct here — a round needle laying a round bead has no meaningful
|
|
46
|
+
roll, so pinning one would be inventing a constraint.
|
|
47
|
+
"""
|
|
48
|
+
a = np.asarray(reference, dtype=float)
|
|
49
|
+
b = np.asarray(u, dtype=float)
|
|
50
|
+
na, nb = np.linalg.norm(a), np.linalg.norm(b)
|
|
51
|
+
if na < 1e-15 or nb < 1e-15:
|
|
52
|
+
raise ValueError("rotvec_from_axis needs two non-zero vectors")
|
|
53
|
+
a, b = a / na, b / nb
|
|
54
|
+
c = np.cross(a, b)
|
|
55
|
+
s = float(np.linalg.norm(c))
|
|
56
|
+
d = float(np.dot(a, b))
|
|
57
|
+
if s < 1e-12:
|
|
58
|
+
if d > 0:
|
|
59
|
+
return np.zeros(3)
|
|
60
|
+
# Antiparallel: any perpendicular axis gives a 180 deg flip.
|
|
61
|
+
perp = np.array([1.0, 0.0, 0.0])
|
|
62
|
+
if abs(a[0]) > 0.9:
|
|
63
|
+
perp = np.array([0.0, 1.0, 0.0])
|
|
64
|
+
axis = np.cross(a, perp)
|
|
65
|
+
return axis / np.linalg.norm(axis) * np.pi
|
|
66
|
+
return c / s * float(np.arctan2(s, d))
|
|
67
|
+
|
|
68
|
+
|
|
69
|
+
def rpy_from_rotation(R) -> np.ndarray:
|
|
70
|
+
"""(roll, pitch, yaw) in radians from a rotation matrix, ZYX convention.
|
|
71
|
+
|
|
72
|
+
Yaw about Z, then pitch about the new Y, then roll about the new X — the
|
|
73
|
+
convention robot controllers and teach pendants report, so the numbers here
|
|
74
|
+
match what an operator reads off the machine.
|
|
75
|
+
|
|
76
|
+
At pitch = +/-90 deg roll and yaw describe the same rotation (gimbal lock);
|
|
77
|
+
there the split is arbitrary and roll is pinned to 0 so the result stays
|
|
78
|
+
deterministic rather than amplifying numerical noise.
|
|
79
|
+
"""
|
|
80
|
+
R = np.asarray(R, dtype=float)
|
|
81
|
+
sp = -R[2, 0]
|
|
82
|
+
if abs(sp) > 1.0 - 1e-9: # gimbal lock
|
|
83
|
+
pitch = np.pi / 2 * np.sign(sp)
|
|
84
|
+
return np.array([0.0, pitch, float(np.arctan2(-R[0, 1], R[1, 1]))])
|
|
85
|
+
return np.array([float(np.arctan2(R[2, 1], R[2, 2])),
|
|
86
|
+
float(np.arcsin(np.clip(sp, -1.0, 1.0))),
|
|
87
|
+
float(np.arctan2(R[1, 0], R[0, 0]))])
|
|
88
|
+
|
|
89
|
+
|
|
90
|
+
def _seg_point_distance(pts: np.ndarray, a: np.ndarray, b: np.ndarray) -> np.ndarray:
|
|
91
|
+
"""Distance from each point to the SEGMENT ab (not the infinite line)."""
|
|
92
|
+
pts = np.atleast_2d(pts)
|
|
93
|
+
ab = b - a
|
|
94
|
+
denom = float(ab @ ab)
|
|
95
|
+
if denom < 1e-18: # degenerate segment -> a point
|
|
96
|
+
return np.linalg.norm(pts - a, axis=1)
|
|
97
|
+
t = np.clip((pts - a) @ ab / denom, 0.0, 1.0)
|
|
98
|
+
closest = a + t[:, None] * ab
|
|
99
|
+
return np.linalg.norm(pts - closest, axis=1)
|
|
100
|
+
|
|
101
|
+
|
|
102
|
+
class Capsule:
|
|
103
|
+
"""A cylinder with hemispherical caps: the needle, or a deposited bead.
|
|
104
|
+
|
|
105
|
+
Defined by its axis segment and a radius. A capsule is used rather than a
|
|
106
|
+
plain cylinder because the distance function stays exact and smooth at the
|
|
107
|
+
ends, which matters when a bead is one short segment.
|
|
108
|
+
"""
|
|
109
|
+
|
|
110
|
+
__slots__ = ("a", "b", "radius")
|
|
111
|
+
|
|
112
|
+
def __init__(self, a, b, radius: float):
|
|
113
|
+
self.a = np.asarray(a, dtype=float)
|
|
114
|
+
self.b = np.asarray(b, dtype=float)
|
|
115
|
+
self.radius = float(radius)
|
|
116
|
+
if self.a.shape != (3,) or self.b.shape != (3,):
|
|
117
|
+
raise ValueError("capsule endpoints must be 3-vectors")
|
|
118
|
+
if self.radius < 0:
|
|
119
|
+
raise ValueError("capsule radius must be >= 0")
|
|
120
|
+
|
|
121
|
+
def distance(self, pts) -> np.ndarray:
|
|
122
|
+
"""Signed distance from points to the capsule surface (negative = inside)."""
|
|
123
|
+
return _seg_point_distance(pts, self.a, self.b) - self.radius
|
|
124
|
+
|
|
125
|
+
@property
|
|
126
|
+
def length(self) -> float:
|
|
127
|
+
return float(np.linalg.norm(self.b - self.a))
|
|
128
|
+
|
|
129
|
+
def bounds(self, pad: float = 0.0):
|
|
130
|
+
lo = np.minimum(self.a, self.b) - self.radius - pad
|
|
131
|
+
hi = np.maximum(self.a, self.b) + self.radius + pad
|
|
132
|
+
return lo, hi
|
|
133
|
+
|
|
134
|
+
def __repr__(self):
|
|
135
|
+
return f"Capsule(len={self.length:.4g}, r={self.radius:.4g})"
|
|
136
|
+
|
|
137
|
+
|
|
138
|
+
class Box:
|
|
139
|
+
"""An oriented box — the end-effector housing.
|
|
140
|
+
|
|
141
|
+
`R` maps BOX-local coordinates into world. Distance is exact for points
|
|
142
|
+
outside; for points inside it returns a negative penetration depth (the
|
|
143
|
+
distance to the nearest face), which is the usual convention and is enough
|
|
144
|
+
to rank how bad a collision is.
|
|
145
|
+
"""
|
|
146
|
+
|
|
147
|
+
__slots__ = ("center", "half", "R")
|
|
148
|
+
|
|
149
|
+
def __init__(self, center, half_extents, R=None):
|
|
150
|
+
self.center = np.asarray(center, dtype=float)
|
|
151
|
+
self.half = np.asarray(half_extents, dtype=float)
|
|
152
|
+
self.R = np.eye(3) if R is None else np.asarray(R, dtype=float)
|
|
153
|
+
if self.center.shape != (3,) or self.half.shape != (3,):
|
|
154
|
+
raise ValueError("box center and half_extents must be 3-vectors")
|
|
155
|
+
if np.any(self.half < 0):
|
|
156
|
+
raise ValueError("box half_extents must be >= 0")
|
|
157
|
+
|
|
158
|
+
def distance(self, pts) -> np.ndarray:
|
|
159
|
+
pts = np.atleast_2d(np.asarray(pts, dtype=float))
|
|
160
|
+
local = (pts - self.center) @ self.R # world -> box frame
|
|
161
|
+
excess = np.abs(local) - self.half
|
|
162
|
+
outside = np.linalg.norm(np.maximum(excess, 0.0), axis=1)
|
|
163
|
+
# Inside on every axis -> the closest face is the least-negative excess.
|
|
164
|
+
inside = np.minimum(excess.max(axis=1), 0.0)
|
|
165
|
+
return outside + inside
|
|
166
|
+
|
|
167
|
+
def corners(self) -> np.ndarray:
|
|
168
|
+
"""The 8 world-space corners, for drawing a wireframe."""
|
|
169
|
+
signs = np.array([[sx, sy, sz]
|
|
170
|
+
for sx in (-1, 1) for sy in (-1, 1) for sz in (-1, 1)],
|
|
171
|
+
dtype=float)
|
|
172
|
+
return self.center + (signs * self.half) @ self.R.T
|
|
173
|
+
|
|
174
|
+
def bounds(self, pad: float = 0.0):
|
|
175
|
+
c = self.corners()
|
|
176
|
+
return c.min(axis=0) - pad, c.max(axis=0) + pad
|
|
177
|
+
|
|
178
|
+
def __repr__(self):
|
|
179
|
+
return f"Box(half={np.round(self.half, 4).tolist()})"
|
toolwake/profile.py
ADDED
|
@@ -0,0 +1,181 @@
|
|
|
1
|
+
"""Tool profiles: a coaxial stack of sections, the way CAM describes a tool.
|
|
2
|
+
|
|
3
|
+
A dispensing needle is not a single cylinder. A luer-lock tip is a thin cannula
|
|
4
|
+
that steps up to a hub several tens of times its diameter, and the hub is what
|
|
5
|
+
actually fouls a tall part — exactly the holder-vs-flute distinction a CAM
|
|
6
|
+
simulation draws.
|
|
7
|
+
|
|
8
|
+
Sections are listed from the TIP upward, each with a length and a radius at
|
|
9
|
+
each end, so a straight cylinder and a taper are the same primitive.
|
|
10
|
+
|
|
11
|
+
For collision each section becomes a capsule at its LARGEST radius. That is
|
|
12
|
+
deliberately conservative: on a bioprinter a false stop costs a reprint and a
|
|
13
|
+
missed collision costs the part and possibly the needle. Tapers can be sliced
|
|
14
|
+
into sub-capsules when the loose fit matters more than the caution.
|
|
15
|
+
"""
|
|
16
|
+
from __future__ import annotations
|
|
17
|
+
|
|
18
|
+
from dataclasses import dataclass
|
|
19
|
+
|
|
20
|
+
import numpy as np
|
|
21
|
+
|
|
22
|
+
from .geometry import Capsule
|
|
23
|
+
|
|
24
|
+
__all__ = ["Section", "ToolProfile", "luer_taper_tip", "blunt_cannula"]
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
@dataclass(frozen=True)
|
|
28
|
+
class Section:
|
|
29
|
+
"""One coaxial piece of the tool.
|
|
30
|
+
|
|
31
|
+
Args:
|
|
32
|
+
length: axial length, metres.
|
|
33
|
+
r0: radius at the lower (tip-ward) end, metres.
|
|
34
|
+
r1: radius at the upper end, metres. Defaults to `r0` (a cylinder).
|
|
35
|
+
name: label, used in reports so a collision says which part hit.
|
|
36
|
+
"""
|
|
37
|
+
|
|
38
|
+
length: float
|
|
39
|
+
r0: float
|
|
40
|
+
r1: float | None = None
|
|
41
|
+
name: str = ""
|
|
42
|
+
|
|
43
|
+
@property
|
|
44
|
+
def r_top(self) -> float:
|
|
45
|
+
return self.r0 if self.r1 is None else self.r1
|
|
46
|
+
|
|
47
|
+
@property
|
|
48
|
+
def r_max(self) -> float:
|
|
49
|
+
return max(self.r0, self.r_top)
|
|
50
|
+
|
|
51
|
+
def __post_init__(self):
|
|
52
|
+
if self.length < 0:
|
|
53
|
+
raise ValueError(f"section {self.name!r}: length must be >= 0")
|
|
54
|
+
if self.r0 < 0 or self.r_top < 0:
|
|
55
|
+
raise ValueError(f"section {self.name!r}: radii must be >= 0")
|
|
56
|
+
|
|
57
|
+
|
|
58
|
+
class ToolProfile:
|
|
59
|
+
"""An ordered stack of sections, tip first.
|
|
60
|
+
|
|
61
|
+
The profile is expressed in the TOOL frame with +Z running back up the tool
|
|
62
|
+
away from the tip, and placed into the world by `capsules()`.
|
|
63
|
+
"""
|
|
64
|
+
|
|
65
|
+
def __init__(self, sections):
|
|
66
|
+
self.sections = list(sections)
|
|
67
|
+
if not self.sections:
|
|
68
|
+
raise ValueError("a tool profile needs at least one section")
|
|
69
|
+
|
|
70
|
+
# ------------------------------------------------------------- geometry
|
|
71
|
+
@property
|
|
72
|
+
def length(self) -> float:
|
|
73
|
+
return float(sum(s.length for s in self.sections))
|
|
74
|
+
|
|
75
|
+
@property
|
|
76
|
+
def r_max(self) -> float:
|
|
77
|
+
return float(max(s.r_max for s in self.sections))
|
|
78
|
+
|
|
79
|
+
@property
|
|
80
|
+
def tip_radius(self) -> float:
|
|
81
|
+
"""Radius right at the tip — what lays the bead."""
|
|
82
|
+
return float(self.sections[0].r0)
|
|
83
|
+
|
|
84
|
+
def stations(self) -> np.ndarray:
|
|
85
|
+
"""(M, 2) of (z, r) along the silhouette, from tip upward.
|
|
86
|
+
|
|
87
|
+
A step between sections appears as two samples at the same z, so the
|
|
88
|
+
outline drawn from this has square shoulders rather than ramps.
|
|
89
|
+
"""
|
|
90
|
+
z, out = 0.0, [(0.0, self.sections[0].r0)]
|
|
91
|
+
for s in self.sections:
|
|
92
|
+
if abs(s.r0 - out[-1][1]) > 1e-12: # step change in radius
|
|
93
|
+
out.append((z, s.r0))
|
|
94
|
+
z += s.length
|
|
95
|
+
out.append((z, s.r_top))
|
|
96
|
+
return np.asarray(out, dtype=float)
|
|
97
|
+
|
|
98
|
+
def capsules(self, tip, axis, slices: int = 1):
|
|
99
|
+
"""World-space capsules for collision, one (or `slices`) per section.
|
|
100
|
+
|
|
101
|
+
Args:
|
|
102
|
+
tip: world position of the tool tip.
|
|
103
|
+
axis: unit vector pointing from the tip back up the tool.
|
|
104
|
+
slices: subdivisions per tapered section. 1 keeps each section at
|
|
105
|
+
its largest radius (conservative); higher follows the taper
|
|
106
|
+
more tightly at proportional cost.
|
|
107
|
+
|
|
108
|
+
Returns:
|
|
109
|
+
list of (name, Capsule).
|
|
110
|
+
"""
|
|
111
|
+
tip = np.asarray(tip, dtype=float)
|
|
112
|
+
axis = np.asarray(axis, dtype=float)
|
|
113
|
+
axis = axis / np.linalg.norm(axis)
|
|
114
|
+
out, z = [], 0.0
|
|
115
|
+
for s in self.sections:
|
|
116
|
+
if s.length <= 0:
|
|
117
|
+
continue
|
|
118
|
+
n = max(1, int(slices)) if s.r1 is not None and s.r1 != s.r0 else 1
|
|
119
|
+
for k in range(n):
|
|
120
|
+
t0, t1 = k / n, (k + 1) / n
|
|
121
|
+
za, zb = z + s.length * t0, z + s.length * t1
|
|
122
|
+
ra = s.r0 + (s.r_top - s.r0) * t0
|
|
123
|
+
rb = s.r0 + (s.r_top - s.r0) * t1
|
|
124
|
+
out.append((s.name or "section",
|
|
125
|
+
Capsule(tip + axis * za, tip + axis * zb,
|
|
126
|
+
max(ra, rb))))
|
|
127
|
+
z += s.length
|
|
128
|
+
return out
|
|
129
|
+
|
|
130
|
+
def surface(self, tip, R, n_theta: int = 24):
|
|
131
|
+
"""(X, Y, Z) arrays for drawing the tool as a surface of revolution."""
|
|
132
|
+
st = self.stations()
|
|
133
|
+
th = np.linspace(0.0, 2 * np.pi, n_theta)
|
|
134
|
+
zz = st[:, 0][:, None]
|
|
135
|
+
rr = st[:, 1][:, None]
|
|
136
|
+
local = np.stack([rr * np.cos(th), rr * np.sin(th),
|
|
137
|
+
np.broadcast_to(zz, (len(st), n_theta))], axis=-1)
|
|
138
|
+
world = local @ R.T + np.asarray(tip, dtype=float)
|
|
139
|
+
return world[..., 0], world[..., 1], world[..., 2]
|
|
140
|
+
|
|
141
|
+
def __repr__(self):
|
|
142
|
+
names = ", ".join(s.name or "?" for s in self.sections)
|
|
143
|
+
return (f"ToolProfile({len(self.sections)} sections: {names}; "
|
|
144
|
+
f"len={self.length*1e3:.1f} mm, r_max={self.r_max*1e3:.2f} mm)")
|
|
145
|
+
|
|
146
|
+
|
|
147
|
+
# ------------------------------------------------------------------ presets
|
|
148
|
+
#
|
|
149
|
+
# NOMINAL dimensions. Hub sizes in particular vary between manufacturers and
|
|
150
|
+
# these are close enough to reason about clearance but not a substitute for
|
|
151
|
+
# calipers on the tip you are actually running.
|
|
152
|
+
|
|
153
|
+
def blunt_cannula(gauge_od=0.19e-3, length=12.7e-3, inner_d=90e-6) -> ToolProfile:
|
|
154
|
+
"""Just the metal tube — no hub. The optimistic model, for comparison."""
|
|
155
|
+
return ToolProfile([Section(length, gauge_od / 2.0, name="cannula")])
|
|
156
|
+
|
|
157
|
+
|
|
158
|
+
def luer_taper_tip(cannula_od=0.19e-3, cannula_len=12.7e-3,
|
|
159
|
+
hub_d=9.0e-3, hub_len=13.0e-3,
|
|
160
|
+
taper_len=3.0e-3, collar_d=11.0e-3,
|
|
161
|
+
collar_len=4.0e-3) -> ToolProfile:
|
|
162
|
+
"""A luer-lock dispensing tip: cannula, taper, hub, locking collar.
|
|
163
|
+
|
|
164
|
+
Defaults approximate a 34G half-inch tip. The hub is roughly 47x the
|
|
165
|
+
cannula diameter, which is the entire reason this class exists — a check
|
|
166
|
+
that models only the cannula is looking at the thinnest 2% of the tool.
|
|
167
|
+
|
|
168
|
+
Args:
|
|
169
|
+
cannula_od: needle outer diameter, metres (34G ~ 0.19 mm).
|
|
170
|
+
cannula_len: exposed needle length, metres (0.5 in = 12.7 mm).
|
|
171
|
+
hub_d: widest diameter of the plastic hub, metres.
|
|
172
|
+
hub_len: straight hub length, metres.
|
|
173
|
+
taper_len: cone joining cannula to hub, metres.
|
|
174
|
+
collar_d, collar_len: the luer locking collar above the hub.
|
|
175
|
+
"""
|
|
176
|
+
return ToolProfile([
|
|
177
|
+
Section(cannula_len, cannula_od / 2.0, name="cannula"),
|
|
178
|
+
Section(taper_len, cannula_od / 2.0, hub_d / 2.0, name="taper"),
|
|
179
|
+
Section(hub_len, hub_d / 2.0, name="hub"),
|
|
180
|
+
Section(collar_len, collar_d / 2.0, name="luer collar"),
|
|
181
|
+
])
|