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