avoid.py

from mpl_toolkits.mplot3d import Axes3D
from matplotlib import pyplot as plt
import numpy as np
try:
    import Queue
except:
    import queue as Queue

# total time
T = 3
# interval time
DT = 0.1
# Update Time
UT = int(T / DT)
# MIN MIN_TOLERANCE DISTANCE
MIN_TOLERANCE = 0.5

class Obstacle:
    def __init__(self, center, r, v):
        self.center = np.array(center)
        self.r = r
        self.v = v
    def draw(self, ax):
        r = np.linspace(self.r,  self.r + T * self.v, UT)
        a = np.linspace(0, 2 * np.pi, UT) 
        R, A = np.meshgrid(r, a) 
        Z = (r - self.r) / self.v
        X, Y = R * np.cos(A) + self.center[0], R * np.sin(A) + self.center[1]
        ax.plot_surface(X, Y, Z, cmap=plt.cm.Reds)
    def is_crash(self, pos, t):
        r = t * self.v + self.r
        d = pos - self.center 
        distance = np.sqrt(np.sum(np.square(d)))
        return distance <= r + MIN_TOLERANCE

def get_dis2(a, b):
    d = np.array(a) - np.array(b)
    return np.sqrt(np.sum(np.square(d)))

class Plane:
    def __init__(self, center, v, target):
        self.center = np.array(center)
        self.v = v
        self.target = np.array(target)
    def draw(self, ax, obstacles):
        for o in obstacles:
            if o.is_crash(self.center, 0):
                print ("ERROR CENTER")
                return
        for o in obstacles:
            if o.is_crash(self.target, T):
                print ("ERROR TARGET")
                return
        print ("CHECK OK")
        def W(v):
            return (v[0], v[1])
        def CT(c, t):
            return (c[0], c[1], t)
        q = Queue.PriorityQueue()
        q.put((0, get_dis2(self.center, self.target), W(self.center)))
        parent = dict({CT(self.center, 0): None})
        sa = np.linspace(0, 2 * np.pi, 8)
        found = False
        npos = None
        while not q.empty() and not found:
            t, d, pos = q.get()
            nt = t + DT 
            if nt > T:
                continue
            for a in sa:
                npos = pos + np.array(np.cos(a), np.sin(a)) * self.v
                for o in obstacles:
                    if o.is_crash(npos, nt):
                        break
                else:
                    nd = get_dis2(pos, self.target) 
                    print (nd, pos)
                    q.put((nt, nd, W(npos)))
                    parent[CT(npos, nt)] = CT(pos, t)
                    if nd < self.v * DT: 
                        found = True
                        npos = CT(npos, nt)
                        break
        if not found:
            print ("Path Not Found")
            return
        xs = []
        ys = []
        zs = []
        while parent[npos] is not None:
            print (npos)
            xs.append(npos[0])
            ys.append(npos[1])
            zs.append(npos[2])
            npos = parent[npos]

        for i in range(len(xs) - 1):
            ax.plot([xs[i], xs[i + 1]], [ys[i], ys[i + 1]], [zs[i], zs[i + 1]])#, cmap = plt.cm.Blues)




fig = plt.figure("equal")
ax = Axes3D(fig)
plt.xlim(-10, 10)
plt.ylim(-10, 10)

obstacles = [Obstacle((0, 1), 0.33, 0.33), Obstacle((1, 0), 0.33, 0.33)]

for o in obstacles:
    o.draw(ax)
plane = Plane(center = (0,0), v = 1.5, target = (0,3))
plane.draw(ax, obstacles) 

plt.show()