From 2ce2a01b243a4d2ede1b9a6f841b82112a8b02b4 Mon Sep 17 00:00:00 2001
From: 391311qy <391311qy@gmail.com>
Date: Thu, 25 Jun 2020 18:55:55 -0700
Subject: [PATCH] rrt3D fin, prep A*

---
 .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 5347 -> 5349 bytes
 Sampling-based Planning/rrt_3D/rrtstar3D.py   |  28 ++--
 Sampling-based Planning/rrt_3D/utils3D.py     |   2 +-
 Search-based Planning/3D/env3D.py             |  49 +++++++
 Search-based Planning/3D/plot_util3D.py       | 137 ++++++++++++++++++
 5 files changed, 198 insertions(+), 18 deletions(-)
 create mode 100644 Search-based Planning/3D/env3D.py
 create mode 100644 Search-based Planning/3D/plot_util3D.py

diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
index e80d2483c6b7ac7bc7c1caf38f06b5b8cd8d4ce2..530e52aed392b15bbdcdd71df5b2a33e22f530fa 100644
GIT binary patch
delta 52
zcmaE?`BanFiI<m)0SIn~e2v?_k++bWamwU+?f~wn8ip*!6edZAbjHm$xLFt(c{bnW
HVdDS*o3svy

delta 50
zcmaE=`B;<JiI<m)0SGSJe~sI^k++bWanj^^?f}k+8ip*!6edZA&9}H&7#X=X-{)cD
F004WH4gCNB

diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py
index ef08f55..dadc702 100644
--- a/Sampling-based Planning/rrt_3D/rrtstar3D.py	
+++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py	
@@ -65,23 +65,17 @@ class rrtstar():
                 # 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)
+                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
             self.ind += 1
         # max sample reached
diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py
index 7a984c3..75ee073 100644
--- a/Sampling-based Planning/rrt_3D/utils3D.py	
+++ b/Sampling-based Planning/rrt_3D/utils3D.py	
@@ -102,7 +102,7 @@ def near(initparams, x):
     if initparams.done: r = 1
     V = np.array(initparams.V)
     if initparams.i == 0:
-        return initparams.V[0]
+        return [initparams.V[0]]
     xr = repmat(x, len(V), 1)
     inside = np.linalg.norm(xr - V, axis=1) < r
     nearpoints = V[inside]
diff --git a/Search-based Planning/3D/env3D.py b/Search-based Planning/3D/env3D.py
new file mode 100644
index 0000000..977aa8d
--- /dev/null
+++ b/Search-based Planning/3D/env3D.py	
@@ -0,0 +1,49 @@
+# 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([j/resolution for j in i])
+    return np.array(Obstacles)
+
+def getballs(resolution):
+    spheres = [[16,2.5,3,2],[10,2.5,1,1]]
+    Obstacles = []
+    for i in spheres:
+        Obstacles.append([j/resolution for j in i])
+    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.balls = getballs(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()
+    print(newenv.balls)
diff --git a/Search-based Planning/3D/plot_util3D.py b/Search-based Planning/3D/plot_util3D.py
new file mode 100644
index 0000000..b164fd6
--- /dev/null
+++ b/Search-based Planning/3D/plot_util3D.py	
@@ -0,0 +1,137 @@
+# 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
+from mpl_toolkits.mplot3d import proj3d
+import numpy as np
+
+def CreateSphere(center,r):
+    u = np.linspace(0,2* np.pi,30)
+    v = np.linspace(0,np.pi,30)
+    x = np.outer(np.cos(u),np.sin(v))
+    y = np.outer(np.sin(u),np.sin(v))
+    z = np.outer(np.ones(np.size(u)),np.cos(v))
+    x, y, z = r*x + center[0], r*y + center[1], r*z + center[2]
+    return (x,y,z)
+
+def draw_Spheres(ax,balls):
+    for i in balls:
+        (xs,ys,zs) = CreateSphere(i[0:3],i[-1])
+        ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b")
+
+def draw_block_list(ax, blocks ,color=None,alpha=0.15):
+    '''
+    drawing the blocks on the graph
+    '''
+    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]])
+    n = blocks.shape[0]
+    d = blocks[:, 3:6] - blocks[:, :3]
+    vl = np.zeros((8 * n, 3))
+    fl = np.zeros((6 * n, 4), dtype='int64')
+    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
+    if type(ax) is Poly3DCollection:
+        ax.set_verts(vl[fl])
+    else:
+        pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
+        pc.set_facecolor(color)
+        h = ax.add_collection3d(pc)
+        return h
+
+def draw_line(ax,SET,visibility=1,color=None):
+    if SET != []:
+        for i in SET:
+            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, alpha=visibility, color=color)
+            ax.add_line(line)
+
+def visualization(initparams):
+    if initparams.ind % 10 == 0 or initparams.done:
+        V = np.array(initparams.V)
+        E = initparams.E
+        Path = np.array(initparams.Path)
+        start = initparams.env.start
+        goal = initparams.env.goal
+        edges = E.get_edge()
+        # generate axis objects
+        ax = plt.subplot(111, projection='3d')
+        ax.view_init(elev=0., azim=90)
+        ax.clear()
+        # drawing objects
+        draw_Spheres(ax, initparams.env.balls)
+        draw_block_list(ax, initparams.env.blocks)
+        draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)
+        draw_line(ax,edges,visibility=0.25)
+        draw_line(ax,Path,color='r')
+        ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
+        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') 
+        # adjust the aspect ratio
+        xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3]
+        ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4]
+        zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5]
+        dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin
+        ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy)
+        plt.xlabel('x')
+        plt.ylabel('y')
+        plt.pause(0.001)
+
+def make_get_proj(self, rx, ry, rz):
+    '''
+    Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that
+    makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*.
+    '''
+
+    rm = max(rx, ry, rz)
+    kx = rm / rx; ky = rm / ry; kz = rm / rz
+
+    # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are
+    # marked by ##
+    def get_proj():
+        relev, razim = np.pi * self.elev/180, np.pi * self.azim/180
+
+        xmin, xmax = self.get_xlim3d()
+        ymin, ymax = self.get_ylim3d()
+        zmin, zmax = self.get_zlim3d()
+
+        # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0
+        worldM = proj3d.world_transformation(xmin, xmax,
+                                             ymin, ymax,
+                                             zmin, zmax)
+        ratio = 0.5
+        # adjust the aspect ratio                          ##
+        aspectM = proj3d.world_transformation(-kx + 1, kx, ##
+                                              -ky + 1, ky, ##
+                                              -kz + 1, kz) ##
+
+        # look into the middle of the new coordinates
+        R = np.array([0.5, 0.5, 0.5])
+
+        xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist *ratio
+        yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist *ratio
+        zp = R[2] + np.sin(relev) * self.dist *ratio
+        E = np.array((xp, yp, zp))
+
+        self.eye = E
+        self.vvec = R - E
+        self.vvec = self.vvec / np.linalg.norm(self.vvec)
+
+        if abs(relev) > np.pi/2:
+            # upside down
+            V = np.array((0, 0, -1))
+        else:
+            V = np.array((0, 0, 1))
+        zfront, zback = -self.dist *ratio, self.dist *ratio
+
+        viewM = proj3d.view_transformation(E, R, V)
+        perspM = proj3d.persp_transformation(zfront, zback)
+        M0 = np.dot(viewM, np.dot(aspectM, worldM)) ##
+        M = np.dot(perspM, M0)
+        return M
+    return get_proj
\ No newline at end of file