"""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))