diff --git a/Sampling-based Planning/RRT*.py b/Sampling-based Planning/RRT*.py
new file mode 100644
index 0000000..c4d1167
--- /dev/null
+++ b/Sampling-based Planning/RRT*.py
@@ -0,0 +1,200 @@
+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)
diff --git a/Sampling-based Planning/RRT.py b/Sampling-based Planning/RRT.py
index 15db973..6052cbb 100644
--- a/Sampling-based Planning/RRT.py
+++ b/Sampling-based Planning/RRT.py
@@ -9,8 +9,6 @@ class Node:
def __init__(self, n):
self.x = n[0]
self.y = n[1]
- self.path_x = []
- self.path_y = []
self.parent = None
@@ -18,7 +16,7 @@ class RRT:
def __init__(self, xI, xG):
self.xI = Node(xI)
self.xG = Node(xG)
- self.expand_len = 0.8
+ self.expand_len = 0.4
self.goal_sample_rate = 0.05
self.iterations = 5000
self.node_list = [self.xI]
@@ -28,27 +26,49 @@ class RRT:
self.x_range = self.env.x_range
self.y_range = self.env.y_range
- self.obs_circle = self.env.obs
- self.obs_rectangle = self.env.obs_boundary
+ 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.generate_random_node()
- node_near = self.get_nearest_node(self.node_list, node_rand)
- node_new = self.new_node(node_near, node_rand, self.expand_len)
+ 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.obs_circle, self.obs_rectangle):
+ if not self.check_collision(node_new):
self.node_list.append(node_new)
- if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_len:
- self.new_node(self.node_list[-1], self.xG, self.expand_len)
+ 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]
@@ -59,62 +79,36 @@ class RRT:
return path
- def cal_dis_to_goal(self, node_cal):
+ def dis_to_goal(self, node_cal):
return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y)
- def new_node(self, node_start, node_goal, expand_len):
- new_node = Node((node_start.x, node_start.y))
- d, theta = self.calc_distance_and_angle(new_node, node_goal)
-
- new_node.path_x = [new_node.x]
- new_node.path_y = [new_node.y]
-
- if d < expand_len:
- expand_len = d
-
- new_node.x += expand_len * math.cos(theta)
- new_node.y += expand_len * math.sin(theta)
- new_node.path_x.append(new_node.x)
- new_node.path_y.append(new_node.y)
-
- new_node.parent = node_start
-
- return new_node
-
- def generate_random_node(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 get_nearest_node(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]))]
-
- @staticmethod
- def calc_distance_and_angle(from_node, to_node):
- dx = to_node.x - from_node.x
- dy = to_node.y - from_node.y
- return math.hypot(dx, dy), math.atan2(dy, dx)
-
- @staticmethod
- def check_collision(node_end, obs_circle, obs_rectangle):
+ def check_collision(self, node_end):
if node_end is None:
return True
- for (ox, oy, r) in obs_circle:
+ 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 obs_rectangle:
+ 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 = (15, 5) # Starting node
- x_Goal = (45, 25) # Goal node
+ x_Start = (2, 2) # Starting node
+ x_Goal = (49, 28) # Goal node
rrt = RRT(x_Start, x_Goal)
diff --git a/Sampling-based Planning/__pycache__/env.cpython-37.pyc b/Sampling-based Planning/__pycache__/env.cpython-37.pyc
index e743b73..1c0531a 100644
Binary files a/Sampling-based Planning/__pycache__/env.cpython-37.pyc and b/Sampling-based Planning/__pycache__/env.cpython-37.pyc differ
diff --git a/Sampling-based Planning/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/__pycache__/env3D.cpython-37.pyc
new file mode 100644
index 0000000..49568f8
Binary files /dev/null and b/Sampling-based Planning/__pycache__/env3D.cpython-37.pyc differ
diff --git a/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc
index 59dd57e..efb6181 100644
Binary files a/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc and b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Sampling-based Planning/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/__pycache__/utils3D.cpython-37.pyc
new file mode 100644
index 0000000..2541f91
Binary files /dev/null and b/Sampling-based Planning/__pycache__/utils3D.cpython-37.pyc differ
diff --git a/Sampling-based Planning/env.py b/Sampling-based Planning/env.py
index 85a0aba..f095a59 100644
--- a/Sampling-based Planning/env.py
+++ b/Sampling-based Planning/env.py
@@ -3,7 +3,8 @@ class Env:
self.x_range = (0, 50)
self.y_range = (0, 30)
self.obs_boundary = self.obs_boundary()
- self.obs = self.obs_circle()
+ self.obs_circle = self.obs_circle()
+ self.obs_rectangle = self.obs_rectangle()
@staticmethod
def obs_boundary():
@@ -11,15 +12,25 @@ class Env:
(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)
+ (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 = [
diff --git a/Sampling-based Planning/plotting.py b/Sampling-based Planning/plotting.py
index de838bc..02b96b7 100644
--- a/Sampling-based Planning/plotting.py
+++ b/Sampling-based Planning/plotting.py
@@ -8,33 +8,44 @@ class Plotting:
self.xI, self.xG = xI, xG
self.env = env.Env()
self.obs_bound = self.env.obs_boundary
- self.obs_circle = self.env.obs
+ self.obs_circle = self.env.obs_circle
+ self.obs_rectangle = self.env.obs_rectangle
- def animation(self, nodelist, path):
+ def animation(self, nodelist, path, animation=False):
if path is None:
print("No path found!")
return
-
- self.plot_visited(nodelist)
+ self.plot_grid("RRT")
+ self.plot_visited(nodelist, animation)
self.plot_path(path)
def plot_grid(self, name):
fig, ax = plt.subplots()
- for x in self.obs_bound:
+ for (ox, oy, w, h) in self.obs_bound:
ax.add_patch(
patches.Rectangle(
- (x[0], x[1]), x[2], x[3],
+ (ox, oy), w, h,
edgecolor='black',
facecolor='black',
fill=True
)
)
- for x in self.obs_circle:
+ 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(
- (x[0], x[1]), x[2],
+ (ox, oy), r,
edgecolor='black',
facecolor='gray',
fill=True
@@ -47,13 +58,19 @@ class Plotting:
plt.axis("equal")
@staticmethod
- def plot_visited(nodelist):
- for node in nodelist:
- if node.parent:
- plt.plot(node.path_x, node.path_y, "-g")
- plt.gcf().canvas.mpl_connect('key_release_event',
- lambda event: [exit(0) if event.key == 'escape' else None])
- plt.pause(0.001)
+ 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):
diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index ac72159..2045d90 100644
--- a/Search-based Planning/.idea/workspace.xml
+++ b/Search-based Planning/.idea/workspace.xml
@@ -20,10 +20,6 @@
-
-
-
-
@@ -53,7 +49,7 @@
-
+
@@ -199,10 +195,11 @@
-
-
+
+
+