diff --git a/Sampling-based Planning/.idea/.gitignore b/Sampling-based Planning/.idea/.gitignore
new file mode 100644
index 0000000..26d3352
--- /dev/null
+++ b/Sampling-based Planning/.idea/.gitignore
@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml
diff --git a/Sampling-based Planning/.idea/Sampling-based Planning.iml b/Sampling-based Planning/.idea/Sampling-based Planning.iml
new file mode 100644
index 0000000..5965bde
--- /dev/null
+++ b/Sampling-based Planning/.idea/Sampling-based Planning.iml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml b/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000..105ce2d
--- /dev/null
+++ b/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/misc.xml b/Sampling-based Planning/.idea/misc.xml
new file mode 100644
index 0000000..0e7ac62
--- /dev/null
+++ b/Sampling-based Planning/.idea/misc.xml
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/modules.xml b/Sampling-based Planning/.idea/modules.xml
new file mode 100644
index 0000000..caf2a07
--- /dev/null
+++ b/Sampling-based Planning/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/vcs.xml b/Sampling-based Planning/.idea/vcs.xml
new file mode 100644
index 0000000..6c0b863
--- /dev/null
+++ b/Sampling-based Planning/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc
new file mode 100644
index 0000000..d6169c0
Binary files /dev/null and b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc
new file mode 100644
index 0000000..c055c0c
Binary files /dev/null and b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/env.py b/Sampling-based Planning/rrt_2D/env.py
new file mode 100644
index 0000000..b8b8e0d
--- /dev/null
+++ b/Sampling-based Planning/rrt_2D/env.py
@@ -0,0 +1,45 @@
+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
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt_2D/plotting.py b/Sampling-based Planning/rrt_2D/plotting.py
new file mode 100644
index 0000000..24ec50b
--- /dev/null
+++ b/Sampling-based Planning/rrt_2D/plotting.py
@@ -0,0 +1,80 @@
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+from rrt_2D 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()
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt_2D/rrt*.py b/Sampling-based Planning/rrt_2D/rrt*.py
new file mode 100644
index 0000000..e105b7f
--- /dev/null
+++ b/Sampling-based Planning/rrt_2D/rrt*.py
@@ -0,0 +1,200 @@
+from rrt_2D import env
+from rrt_2D 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)
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt_2D/rrt.py b/Sampling-based Planning/rrt_2D/rrt.py
new file mode 100644
index 0000000..5609603
--- /dev/null
+++ b/Sampling-based Planning/rrt_2D/rrt.py
@@ -0,0 +1,114 @@
+from rrt_2D import env
+from rrt_2D 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)
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc
new file mode 100644
index 0000000..9aafad5
Binary files /dev/null and b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
new file mode 100644
index 0000000..42deba7
Binary files /dev/null and b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_3D/env3D.py b/Sampling-based Planning/rrt_3D/env3D.py
new file mode 100644
index 0000000..118452b
--- /dev/null
+++ b/Sampling-based Planning/rrt_3D/env3D.py
@@ -0,0 +1,44 @@
+# 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)
diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py
new file mode 100644
index 0000000..0700580
--- /dev/null
+++ b/Sampling-based Planning/rrt_3D/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 rrt_3D.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))
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py
new file mode 100644
index 0000000..bcd6418
--- /dev/null
+++ b/Sampling-based Planning/rrt_3D/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 rrt_3D.env3D import env
+from collections import defaultdict
+import pyrr as pyrr
+from rrt_3D.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))
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py
new file mode 100644
index 0000000..4aa8290
--- /dev/null
+++ b/Sampling-based Planning/rrt_3D/utils3D.py
@@ -0,0 +1,177 @@
+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
+