From a22f3ef98a8713429a42f1903d5563ebbafcd5c2 Mon Sep 17 00:00:00 2001
From: 391311qy <391311qy@gmail.com>
Date: Tue, 23 Jun 2020 18:12:20 -0700
Subject: [PATCH] yue qi update rrt*3d

---
 Sampling-based Planning/env3D.py     |  40 +++++++
 Sampling-based Planning/rrt3D.py     |  59 ++++++++++
 Sampling-based Planning/rrtstar3D.py |  79 +++++++++++++
 Sampling-based Planning/utils3D.py   | 161 +++++++++++++++++++++++++++
 4 files changed, 339 insertions(+)
 create mode 100644 Sampling-based Planning/env3D.py
 create mode 100644 Sampling-based Planning/rrt3D.py
 create mode 100644 Sampling-based Planning/rrtstar3D.py
 create mode 100644 Sampling-based Planning/utils3D.py

diff --git a/Sampling-based Planning/env3D.py b/Sampling-based Planning/env3D.py
new file mode 100644
index 0000000..a9611f3
--- /dev/null
+++ b/Sampling-based Planning/env3D.py	
@@ -0,0 +1,40 @@
+# this is the three dimensional configuration space for rrt
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: yue qi
+"""
+import numpy as np
+
+def getblocks(resolution):
+    # AABBs 
+    block = [[3.10e+00,0.00e+00,2.10e+00,3.90e+00,5.00e+00,6.00e+00],
+            [9.10e+00,0.00e+00,2.10e+00,9.90e+00,5.00e+00,6.00e+00],
+            [1.51e+01,0.00e+00,2.10e+00,1.59e+01,5.00e+00,6.00e+00],
+            [1.00e-01,0.00e+00,0.00e+00,9.00e-01,5.00e+00,3.90e+00],
+            [6.10e+00,0.00e+00,0.00e+00,6.90e+00,5.00e+00,3.90e+00],
+            [1.21e+01,0.00e+00,0.00e+00,1.29e+01,5.00e+00,3.90e+00],
+            [1.81e+01,0.00e+00,0.00e+00,1.89e+01,5.00e+00,3.90e+00]]
+    Obstacles = []
+    for i in block:
+        i = np.array(i)
+        Obstacles.append((i[0]/resolution,i[1]/resolution,i[2]/resolution,i[3]/resolution,i[4]/resolution,i[5]/resolution))
+    return np.array(Obstacles)
+
+class env():
+    def __init__(self,xmin=0,ymin=0,zmin=0,xmax=20,ymax=5,zmax=6,resolution=1):
+        self.resolution = resolution
+        self.boundary = np.array([xmin,ymin,zmin,xmax,ymax,zmax])/resolution
+        self.blocks = getblocks(resolution)
+        self.start = np.array([0.5, 2.5, 5.5])
+        self.goal = np.array([19.0, 2.5, 5.5])
+
+    def visualize(self):
+        # fig = plt.figure()
+        # TODO: do visualizations
+        return
+
+if __name__ == '__main__': 
+    newenv = env()
+    X = StateSpace(newenv.boundary,newenv.resolution)
+    print(X)
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt3D.py b/Sampling-based Planning/rrt3D.py
new file mode 100644
index 0000000..c7045d3
--- /dev/null
+++ b/Sampling-based Planning/rrt3D.py	
@@ -0,0 +1,59 @@
+
+"""
+This is rrt star code for 3D
+@author: yue qi
+"""
+import numpy as np
+from numpy.matlib import repmat
+from env3D import env
+from collections import defaultdict
+import pyrr as pyrr
+from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
+import time
+
+
+class rrtstar():
+    def __init__(self):
+        self.env = env()
+        self.Parent = defaultdict(lambda: defaultdict(dict))
+        self.V = []
+        self.E = []
+        self.i = 0
+        self.maxiter = 10000
+        self.stepsize = 0.5
+        self.Path = []
+
+    def wireup(self,x,y):
+        self.E.append([x,y]) # add edge
+        self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
+
+    def removewire(self,xnear):
+        xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
+        a = np.array([xnear,xparent])
+        self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection 
+
+    def run(self):
+        self.V.append(self.env.start)
+        ind = 0
+        xnew = self.env.start
+        while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
+            xrand    = sampleFree(self)
+            xnearest = nearest(self,xrand)
+            xnew     = steer(self,xnearest,xrand)
+            if not isCollide(self,xnearest,xnew):
+                self.V.append(xnew) # add point
+                self.wireup(xnew,xnearest)
+                #visualization(self)
+                self.i += 1
+            ind += 1
+            if getDist(xnew,self.env.goal) <= 1:
+                self.wireup(self.env.goal,xnew)
+                self.Path,D = path(self)
+                print('Total distance = '+str(D))
+        visualization(self)
+
+if __name__ == '__main__':
+    p = rrtstar()
+    starttime = time.time()
+    p.run()
+    print('time used = ' + str(time.time()-starttime))
diff --git a/Sampling-based Planning/rrtstar3D.py b/Sampling-based Planning/rrtstar3D.py
new file mode 100644
index 0000000..821e2c3
--- /dev/null
+++ b/Sampling-based Planning/rrtstar3D.py	
@@ -0,0 +1,79 @@
+
+"""
+This is rrt star code for 3D
+@author: yue qi
+"""
+import numpy as np
+from numpy.matlib import repmat
+from env3D import env
+from collections import defaultdict
+import pyrr as pyrr
+from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path
+import time
+
+
+class rrtstar():
+    def __init__(self):
+        self.env = env()
+        self.Parent = defaultdict(lambda: defaultdict(dict))
+        self.V = []
+        self.E = []
+        self.i = 0
+        self.maxiter = 10000
+        self.stepsize = 0.5
+        self.Path = []
+
+    def wireup(self,x,y):
+        self.E.append([x,y]) # add edge
+        self.Parent[str(x[0])][str(x[1])][str(x[2])] = y
+
+    def removewire(self,xnear):
+        xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])]
+        a = np.array([xnear,xparent])
+        self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection 
+
+    def run(self):
+        self.V.append(self.env.start)
+        ind = 0
+        xnew = self.env.start
+        while ind < self.maxiter and getDist(xnew,self.env.goal) > 1:
+            xrand    = sampleFree(self)
+            xnearest = nearest(self,xrand)
+            xnew     = steer(self,xnearest,xrand)
+            if not isCollide(self,xnearest,xnew):
+                Xnear = near(self,xnew)
+                self.V.append(xnew) # add point
+                # visualization(self)
+                # minimal path and minimal cost
+                xmin,cmin = xnearest,cost(self,xnearest) + getDist(xnearest,xnew) 
+                # connecting along minimal cost path
+                if self.i == 0: 
+                    c1 = cost(self,Xnear) + getDist(xnew,Xnear)
+                    if not isCollide(self,xnew,Xnear) and c1 < cmin:
+                            xmin,cmin = Xnear,c1
+                    self.wireup(xnew,xmin)
+                else:
+                    for xnear in Xnear: 
+                        c1 = cost(self,xnear) + getDist(xnew,xnear)
+                        if not isCollide(self,xnew,xnear) and c1 < cmin:
+                            xmin,cmin = xnear,c1
+                    self.wireup(xnew,xmin)
+                    # rewire
+                    for xnear in Xnear: 
+                        c2 = cost(self,xnew) + getDist(xnew,xnear)
+                        if not isCollide(self,xnew,xnear) and c2 < cost(self,xnear):
+                            self.removewire(xnear)
+                            self.wireup(xnear,xnew)
+                self.i += 1
+            ind += 1
+            if getDist(xnew,self.env.goal) <= 1:
+                self.wireup(self.env.goal,xnew)
+                self.Path,D = path(self)
+                print('Total distance = '+str(D))
+        visualization(self)
+
+if __name__ == '__main__':
+    p = rrtstar()
+    starttime = time.time()
+    p.run()
+    print('time used = ' + str(time.time()-starttime))
diff --git a/Sampling-based Planning/utils3D.py b/Sampling-based Planning/utils3D.py
new file mode 100644
index 0000000..12200a5
--- /dev/null
+++ b/Sampling-based Planning/utils3D.py	
@@ -0,0 +1,161 @@
+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
+
+