delete

Sampling-based Planning/.idea/.gitignore

@@ -1,3 +0,0 @@
-# Default ignored files
-/shelf/
-/workspace.xml

Sampling-based Planning/.idea/Sampling-based Planning.iml

@@ -1,11 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module type="PYTHON_MODULE" version="4">
-  <component name="NewModuleRootManager">
-    <content url="file://$MODULE_DIR$" />
-    <orderEntry type="inheritedJdk" />
-    <orderEntry type="sourceFolder" forTests="false" />
-  </component>
-  <component name="TestRunnerService">
-    <option name="PROJECT_TEST_RUNNER" value="pytest" />
-  </component>
-</module>

Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml

@@ -1,6 +0,0 @@
-<component name="InspectionProjectProfileManager">
-  <settings>
-    <option name="USE_PROJECT_PROFILE" value="false" />
-    <version value="1.0" />
-  </settings>
-</component>

Sampling-based Planning/.idea/misc.xml

@@ -1,4 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
-</project>

Sampling-based Planning/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/Sampling-based Planning.iml" filepath="$PROJECT_DIR$/.idea/Sampling-based Planning.iml" />
-    </modules>
-  </component>
-</project>

Sampling-based Planning/.idea/vcs.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
-  </component>
-</project>

Sampling-based Planning/rrt_2D/RRT*.py

@@ -1,200 +0,0 @@
-import env
-import plotting
-
-import numpy as np
-import math
-
-
-class Node:
-    def __init__(self, n):
-        self.x = n[0]
-        self.y = n[1]
-        self.cost = 0.0
-        self.parent = None
-
-
-class RRT:
-    def __init__(self, xI, xG):
-        self.xI = Node(xI)
-        self.xG = Node(xG)
-        self.expand_len = 1
-        self.goal_sample_rate = 0.05
-        self.connect_dist = 10
-        self.iterations = 5000
-        self.node_list = [self.xI]
-
-        self.env = env.Env()
-        self.plotting = plotting.Plotting(xI, xG)
-
-        self.x_range = self.env.x_range
-        self.y_range = self.env.y_range
-        self.obs_circle = self.env.obs_circle
-        self.obs_rectangle = self.env.obs_rectangle
-        self.obs_boundary = self.env.obs_boundary
-
-        self.path = self.planning()
-        self.plotting.animation(self.node_list, self.path, False)
-
-    def planning(self):
-        for k in range(self.iterations):
-            node_rand = self.random_state()
-            node_near = self.nearest_neighbor(self.node_list, node_rand)
-            node_new = self.new_state(node_near, node_rand)
-
-            if not self.check_collision(node_new):
-                neighbor_index = self.find_near_neighbor(node_new)
-                node_new = self.choose_parent(node_new, neighbor_index)
-                if node_new:
-                    self.node_list.append(node_new)
-                    self.rewire(node_new, neighbor_index)
-
-            # if self.dis_to_goal(self.node_list[-1]) <= self.expand_len:
-            #     self.new_state(self.node_list[-1], self.xG)
-            #     return self.extract_path()
-
-        index = self.search_best_goal_node()
-        self.xG.parent = self.node_list[index]
-        return self.extract_path()
-
-    def random_state(self):
-        if np.random.random() > self.goal_sample_rate:
-            return Node((np.random.uniform(self.x_range[0], self.x_range[1]),
-                         np.random.uniform(self.y_range[0], self.y_range[1])))
-        return self.xG
-
-    def nearest_neighbor(self, node_list, n):
-        return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
-                                             for nd in node_list]))]
-
-    def new_state(self, node_start, node_goal):
-        node_new = Node((node_start.x, node_start.y))
-        dist, theta = self.get_distance_and_angle(node_new, node_goal)
-        dist = min(self.expand_len, dist)
-
-        node_new.x += dist * math.cos(theta)
-        node_new.y += dist * math.sin(theta)
-        node_new.parent = node_start
-
-        return node_new
-
-    def find_near_neighbor(self, node_new):
-        n = len(self.node_list) + 1
-        r = min(self.connect_dist * math.sqrt((math.log(n) / n)), self.expand_len)
-
-        dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.node_list]
-        node_index = [dist_table.index(d) for d in dist_table if d <= r]
-
-        return node_index
-
-    def choose_parent(self, node_new, neighbor_index):
-        if not neighbor_index:
-            return None
-
-        cost = []
-
-        for i in neighbor_index:
-            node_near = self.node_list[i]
-            node_mid = self.new_state(node_near, node_new)
-
-            if node_mid and not self.check_collision(node_mid):
-                cost.append(self.update_cost(node_near, node_mid))
-            else:
-                cost.append(float("inf"))
-
-        if min(cost) != float('inf'):
-            index = int(np.argmin(cost))
-            neighbor_min = neighbor_index[index]
-            node_new = self.new_state(self.node_list[neighbor_min], node_new)
-            node_new.cost = min(cost)
-            return node_new
-
-        return None
-
-    def search_best_goal_node(self):
-        dist_to_goal_list = [self.dis_to_goal(n) for n in self.node_list]
-        goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_len]
-
-        return goal_inds[0]
-        # safe_goal_inds = []
-        # for goal_ind in goal_inds:
-        #     t_node = self.new_state(self.node_list[goal_ind], self.xG)
-        #     if self.check_collision(t_node):
-        #         safe_goal_inds.append(goal_ind)
-        #
-        # if not safe_goal_inds:
-        #     print('hahhah')
-        #     return None
-        #
-        # min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
-        # for i in safe_goal_inds:
-        #     if self.node_list[i].cost == min_cost:
-        #         self.xG.parent = self.node_list[i]
-
-    def rewire(self, node_new, neighbor_index):
-        for i in neighbor_index:
-            node_near = self.node_list[i]
-            node_edge = self.new_state(node_new, node_near)
-            if not node_edge:
-                continue
-
-            node_edge.cost = self.update_cost(node_new, node_near)
-            collision = self.check_collision(node_edge)
-            improved_cost = node_near.cost > node_edge.cost
-
-            if not collision and improved_cost:
-                self.node_list[i] = node_edge
-                self.propagate_cost_to_leaves(node_new)
-
-    def update_cost(self, node_start, node_end):
-        dist, theta = self.get_distance_and_angle(node_start, node_end)
-        return node_start.cost + dist
-
-    def propagate_cost_to_leaves(self, parent_node):
-        for node in self.node_list:
-            if node.parent == parent_node:
-                node.cost = self.update_cost(parent_node, node)
-                self.propagate_cost_to_leaves(node)
-
-    def extract_path(self):
-        path = [[self.xG.x, self.xG.y]]
-        node = self.xG
-        while node.parent is not None:
-            path.append([node.x, node.y])
-            node = node.parent
-        path.append([node.x, node.y])
-
-        return path
-
-    def dis_to_goal(self, node_cal):
-        return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
-
-    def check_collision(self, node_end):
-        if node_end is None:
-            return True
-
-        for (ox, oy, r) in self.obs_circle:
-            if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
-                return True
-
-        for (ox, oy, w, h) in self.obs_rectangle:
-            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
-                return True
-
-        for (ox, oy, w, h) in self.obs_boundary:
-            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
-                return True
-
-        return False
-
-    @staticmethod
-    def get_distance_and_angle(node_start, node_end):
-        dx = node_end.x - node_start.x
-        dy = node_end.y - node_start.y
-        return math.hypot(dx, dy), math.atan2(dy, dx)
-
-
-if __name__ == '__main__':
-    x_Start = (2, 2)  # Starting node
-    x_Goal = (49, 28)  # Goal node
-
-    rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/rrt_2D/RRT.py

@@ -1,114 +0,0 @@
-import env
-import plotting
-
-import numpy as np
-import math
-
-
-class Node:
-    def __init__(self, n):
-        self.x = n[0]
-        self.y = n[1]
-        self.parent = None
-
-
-class RRT:
-    def __init__(self, xI, xG):
-        self.xI = Node(xI)
-        self.xG = Node(xG)
-        self.expand_len = 0.4
-        self.goal_sample_rate = 0.05
-        self.iterations = 5000
-        self.node_list = [self.xI]
-
-        self.env = env.Env()
-        self.plotting = plotting.Plotting(xI, xG)
-
-        self.x_range = self.env.x_range
-        self.y_range = self.env.y_range
-        self.obs_circle = self.env.obs_circle
-        self.obs_rectangle = self.env.obs_rectangle
-        self.obs_boundary = self.env.obs_boundary
-
-        self.path = self.planning()
-        self.plotting.animation(self.node_list, self.path)
-
-    def planning(self):
-        for i in range(self.iterations):
-            node_rand = self.random_state()
-            node_near = self.nearest_neighbor(self.node_list, node_rand)
-            node_new = self.new_state(node_near, node_rand)
-
-            if not self.check_collision(node_new):
-                self.node_list.append(node_new)
-
-            if self.dis_to_goal(self.node_list[-1]) <= self.expand_len:
-                self.new_state(self.node_list[-1], self.xG)
-                return self.extract_path(self.node_list)
-
-        return None
-
-    def random_state(self):
-        if np.random.random() > self.goal_sample_rate:
-            return Node((np.random.uniform(self.x_range[0], self.x_range[1]),
-                         np.random.uniform(self.y_range[0], self.y_range[1])))
-        return self.xG
-
-    def nearest_neighbor(self, node_list, n):
-        return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
-                                             for nd in node_list]))]
-
-    def new_state(self, node_start, node_end):
-        node_new = Node((node_start.x, node_start.y))
-        dist, theta = self.get_distance_and_angle(node_new, node_end)
-
-        dist = min(self.expand_len, dist)
-        node_new.x += dist * math.cos(theta)
-        node_new.y += dist * math.sin(theta)
-        node_new.parent = node_start
-
-        return node_new
-
-    def extract_path(self, nodelist):
-        path = [(self.xG.x, self.xG.y)]
-        node_now = nodelist[-1]
-
-        while node_now.parent is not None:
-            node_now = node_now.parent
-            path.append((node_now.x, node_now.y))
-
-        return path
-
-    def dis_to_goal(self, node_cal):
-        return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
-
-    def check_collision(self, node_end):
-        if node_end is None:
-            return True
-
-        for (ox, oy, r) in self.obs_circle:
-            if math.hypot(node_end.x - ox, node_end.y - oy) <= r:
-                return True
-
-        for (ox, oy, w, h) in self.obs_rectangle:
-            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
-                return True
-
-        for (ox, oy, w, h) in self.obs_boundary:
-            if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h:
-                return True
-
-        return False
-
-    @staticmethod
-    def get_distance_and_angle(node_start, node_end):
-        dx = node_end.x - node_start.x
-        dy = node_end.y - node_start.y
-        return math.hypot(dx, dy), math.atan2(dy, dx)
-
-
-if __name__ == '__main__':
-    x_Start = (2, 2)  # Starting node
-    x_Goal = (49, 28)  # Goal node
-
-    rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/rrt_2D/env.py

@@ -1,45 +0,0 @@
-class Env:
-    def __init__(self):
-        self.x_range = (0, 50)
-        self.y_range = (0, 30)
-        self.obs_boundary = self.obs_boundary()
-        self.obs_circle = self.obs_circle()
-        self.obs_rectangle = self.obs_rectangle()
-
-    @staticmethod
-    def obs_boundary():
-        obs_boundary = [
-            (0, 0, 1, 30),
-            (0, 30, 50, 1),
-            (1, 0, 50, 1),
-            (50, 1, 1, 30)
-            # (20, 1, 1, 15),
-            # (10, 15, 10, 1),
-            # (30, 15, 1, 15),
-            # (40, 1, 1, 15)
-        ]
-        return obs_boundary
-
-    @staticmethod
-    def obs_rectangle():
-        obs_rectangle = [
-            (13, 10, 5, 3),
-            (18, 4, 5, 4),
-            (22, 13, 6, 3),
-            (33, 15, 5, 3),
-            (42, 6, 5, 3)
-        ]
-        return obs_rectangle
-
-    @staticmethod
-    def obs_circle():
-        obs_cir = [
-            (5, 10, 3),
-            (10, 22, 3.5),
-            (21, 23, 3),
-            (34, 9, 4),
-            (37, 23, 3),
-            (45, 20, 2)
-        ]
-
-        return obs_cir

Sampling-based Planning/rrt_2D/plotting.py

@@ -1,79 +0,0 @@
-import matplotlib.pyplot as plt
-import matplotlib.patches as patches
-import env
-
-
-class Plotting:
-    def __init__(self, xI, xG):
-        self.xI, self.xG = xI, xG
-        self.env = env.Env()
-        self.obs_bound = self.env.obs_boundary
-        self.obs_circle = self.env.obs_circle
-        self.obs_rectangle = self.env.obs_rectangle
-
-    def animation(self, nodelist, path, animation=False):
-        if path is None:
-            print("No path found!")
-            return
-        self.plot_grid("RRT")
-        self.plot_visited(nodelist, animation)
-        self.plot_path(path)
-
-    def plot_grid(self, name):
-        fig, ax = plt.subplots()
-
-        for (ox, oy, w, h) in self.obs_bound:
-            ax.add_patch(
-                patches.Rectangle(
-                    (ox, oy), w, h,
-                    edgecolor='black',
-                    facecolor='black',
-                    fill=True
-                )
-            )
-
-        for (ox, oy, w, h) in self.obs_rectangle:
-            ax.add_patch(
-                patches.Rectangle(
-                    (ox, oy), w, h,
-                    edgecolor='black',
-                    facecolor='gray',
-                    fill=True
-                )
-            )
-
-        for (ox, oy, r) in self.obs_circle:
-            ax.add_patch(
-                patches.Circle(
-                    (ox, oy), r,
-                    edgecolor='black',
-                    facecolor='gray',
-                    fill=True
-                )
-            )
-
-        plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3)
-        plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3)
-        plt.title(name)
-        plt.axis("equal")
-
-    @staticmethod
-    def plot_visited(nodelist, animation):
-        if animation:
-            for node in nodelist:
-                if node.parent:
-                    plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
-                    plt.gcf().canvas.mpl_connect('key_release_event',
-                                                 lambda event: [exit(0) if event.key == 'escape' else None])
-                    plt.pause(0.001)
-        else:
-            for node in nodelist:
-                if node.parent:
-                    plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
-
-
-    @staticmethod
-    def plot_path(path):
-        plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
-        plt.pause(0.01)
-        plt.show()

Sampling-based Planning/rrt_3D/env3D.py

@@ -1,40 +0,0 @@
-# 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)

Sampling-based Planning/rrt_3D/rrt3D.py

@@ -1,59 +0,0 @@
-
-"""
-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))

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -1,79 +0,0 @@
-
-"""
-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))

Sampling-based Planning/rrt_3D/utils3D.py

@@ -1,161 +0,0 @@
-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
-
-