File size: 2,701 Bytes
81c59cd | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 | """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))
|