Download servo.py from thelamapi/Next-Servo: direct link, hf CLI and curl.
- Browser
- Download file 2.7 kB
-
https://huggingface.co/thelamapi/Next-Servo/resolve/main/servo.py
- Command line
-
hf download hf://thelamapi/Next-Servo/servo.py
-
curl -L -o servo.py https://huggingface.co/thelamapi/Next-Servo/resolve/main/servo.py
2.7 kB
| """Collision-aware planning for a planar two-link arm carrying a disk.""" | |
| from collections import deque | |
| from math import acos, atan2, cos, hypot, pi, sin | |
| STEP = pi / 36 | |
| RADIUS = 0.065 | |
| OFFSETS = ((1, 0), (-1, 0), (0, 1), (0, -1)) | |
| DEMOS = [ | |
| ((.85, .58), (-.63, .27), (.52, .50, .15)), | |
| ((-1.03, .40), (-.11, 1.27), (-.03, .99, .15)), | |
| ((1.39, .49), (-1.02, .53), (.64, 1.28, .15)), | |
| ((-.37, .30), (.27, .89), (.59, .91, .15)), | |
| ] | |
| def forward(q): | |
| a, b = q | |
| elbow = cos(a), sin(a) | |
| return elbow, (elbow[0] + cos(a+b), elbow[1] + sin(a+b)) | |
| def inverse(point): | |
| x, y = point | |
| b = acos((x*x+y*y)/2-1) | |
| for bend in (b, -b): | |
| yield atan2(y, x)-bend/2, bend | |
| def clearance(q, obstacle): | |
| elbow, tip = forward(q) | |
| x, y, radius = obstacle | |
| distances = [] | |
| for (ax, ay), (bx, by) in [((0, 0), elbow), (elbow, tip)]: | |
| dx, dy = bx-ax, by-ay | |
| t = max(0, min(1, (x-ax)*dx+(y-ay)*dy)) | |
| distances.append(hypot(x-ax-t*dx, y-ay-t*dy)-radius) | |
| return min(*distances, hypot(x-tip[0], y-tip[1])-radius-RADIUS) | |
| def interpolate(a, b, t): | |
| return tuple(x+(y-x)*t for x, y in zip(a, b)) | |
| def safe_edge(a, b, obstacle): | |
| # Every point lies within this motion bound of its nearest sample. | |
| margin = (2*abs(b[0]-a[0])+abs(b[1]-a[1])) / 24 | |
| return all(clearance(interpolate(a,b,i/12), obstacle) > margin for i in range(13)) | |
| def plan(start, target, obstacle): | |
| def angles(node): | |
| return tuple(i*STEP for i in node) | |
| def anchors(point): | |
| for q in inverse(point): | |
| i, j = (round(a/STEP) for a in q) | |
| for di in range(-1, 2): | |
| for dj in range(-1, 2): | |
| node = i+di, j+dj | |
| if max(map(abs, node)) <= 36 and safe_edge(q, angles(node), obstacle): | |
| yield node, q | |
| roots, goals = dict(anchors(start)), dict(anchors(target)) | |
| parents = dict.fromkeys(roots) | |
| queue = deque(roots) | |
| while queue: | |
| node = queue.popleft() | |
| if node in goals: | |
| tail = goals[node] | |
| route = [] | |
| while parents[node] is not None: | |
| route.append(angles(node)) | |
| node = parents[node] | |
| return [roots[node], angles(node), *reversed(route), tail] | |
| for di, dj in OFFSETS: | |
| nxt = node[0]+di, node[1]+dj | |
| if (max(map(abs, nxt)) <= 36 and nxt not in parents | |
| and safe_edge(angles(node), angles(nxt), obstacle)): | |
| parents[nxt] = node | |
| queue.append(nxt) | |
| return [] | |
| def direct(start, target, obstacle): | |
| return any(safe_edge(a,b,obstacle) for a in inverse(start) for b in inverse(target)) | |