Path_Planning_Algo_with_Animations/Sampling-based Planning/rrt_3D/utils3D.py

import numpy as np
from numpy.matlib import repmat
import pyrr as pyrr
# plotting
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import mpl_toolkits.mplot3d as plt3d


def getRay(x, y):
    direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
    return np.array([x, direc])


def getAABB(blocks):
    AABB = []
    for i in blocks:
        AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
    return AABB


def getDist(pos1, pos2):
    return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))


def draw_block_list(ax, blocks):
    '''
    Subroutine used by draw_map() to display the environment blocks
    '''
    v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]],
                 dtype='float')
    f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]])
    # clr = blocks[:,6:]/255
    n = blocks.shape[0]
    d = blocks[:, 3:6] - blocks[:, :3]
    vl = np.zeros((8 * n, 3))
    fl = np.zeros((6 * n, 4), dtype='int64')
    # fcl = np.zeros((6*n,3))
    for k in range(n):
        vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3]
        fl[k * 6:(k + 1) * 6, :] = f + k * 8
        # fcl[k*6:(k+1)*6,:] = clr[k,:]

    if type(ax) is Poly3DCollection:
        ax.set_verts(vl[fl])
    else:
        pc = Poly3DCollection(vl[fl], alpha=0.15, linewidths=1, edgecolors='k')
        # pc.set_facecolor(fcl)
        h = ax.add_collection3d(pc)
        return h


''' The following utils can be used for rrt or rrt*,
    required param initparams should have
    env,      environement generated from env3D
    V,        node set
    E,        edge set
    i,        nodes added
    maxiter,  maximum iteration allowed
    stepsize, leaf growth restriction

'''


def sampleFree(initparams):
    x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6])
    if isinside(initparams, x):
        return sampleFree(initparams)
    else:
        return np.array(x)


def isinside(initparams, x):
    '''see if inside obstacle'''
    for i in initparams.env.blocks:
        if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
            return True
    return False


def isCollide(initparams, x, y):
    '''see if line intersects obstacle'''
    ray = getRay(x, y)
    dist = getDist(x, y)
    for i in getAABB(initparams.env.blocks):
        shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
        if shot is not None:
            dist_wall = getDist(x, shot)
            if dist_wall <= dist:  # collide
                return True
    return False


def nearest(initparams, x):
    V = np.array(initparams.V)
    if initparams.i == 0:
        return initparams.V[0]
    xr = repmat(x, len(V), 1)
    dists = np.linalg.norm(xr - V, axis=1)
    return initparams.V[np.argmin(dists)]


def steer(initparams, x, y):
    direc = (y - x) / np.linalg.norm(y - x)
    xnew = x + initparams.stepsize * direc
    return xnew


def near(initparams, x, r=2):
    # TODO: r = min{gamma*log(card(V)/card(V)1/d),eta}
    V = np.array(initparams.V)
    if initparams.i == 0:
        return initparams.V[0]
    xr = repmat(x, len(V), 1)
    inside = np.linalg.norm(xr - V, axis=1) < r
    nearpoints = V[inside]
    return np.array(nearpoints)


def cost(initparams, x):
    '''here use the additive recursive cost function'''
    if all(x == initparams.env.start):
        return 0
    xparent = initparams.Parent[str(x[0])][str(x[1])][str(x[2])]
    return cost(initparams, xparent) + getDist(x, xparent)


def visualization(initparams):
    V = np.array(initparams.V)
    E = np.array(initparams.E)
    Path = np.array(initparams.Path)
    start = initparams.env.start
    goal = initparams.env.goal
    ax = plt.subplot(111, projection='3d')
    ax.view_init(elev=0., azim=90)
    ax.clear()
    draw_block_list(ax, initparams.env.blocks)
    if E != []:
        for i in E:
            xs = i[0][0], i[1][0]
            ys = i[0][1], i[1][1]
            zs = i[0][2], i[1][2]
            line = plt3d.art3d.Line3D(xs, ys, zs)
            ax.add_line(line)

    if Path != []:
        for i in Path:
            xs = i[0][0], i[1][0]
            ys = i[0][1], i[1][1]
            zs = i[0][2], i[1][2]
            line = plt3d.art3d.Line3D(xs, ys, zs, color='r')
            ax.add_line(line)

    ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
    ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
    ax.scatter3D(V[:, 0], V[:, 1], V[:, 2])
    plt.xlim(initparams.env.boundary[0], initparams.env.boundary[3])
    plt.ylim(initparams.env.boundary[1], initparams.env.boundary[4])
    ax.set_zlim(initparams.env.boundary[2], initparams.env.boundary[5])
    plt.xlabel('x')
    plt.ylabel('y')
    if not Path != []:
        plt.pause(0.001)
    else:
        plt.show()


def path(initparams, Path=[], dist=0):
    x = initparams.env.goal
    while not all(x == initparams.env.start):
        x2 = initparams.Parent[str(x[0])][str(x[1])][str(x[2])]
        Path.append(np.array([x, x2]))
        dist += getDist(x, x2)
        x = x2
    return Path, dist