pathplanning 0.1.0__py3-none-any.whl

Sampling_based_Planning/rrt_2D/rrt_connect.py

@@ -0,0 +1,156 @@
+"""
+RRT_CONNECT_2D
+@author: damminhtien
+"""
+
+import os
+import sys
+import math
+import copy
+import numpy as np
+import matplotlib.pyplot as plt
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
+
+
+class RrtConnect:
+    def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max):
+        self.s_start = Node(s_start)
+        self.s_goal = Node(s_goal)
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.iter_max = iter_max
+        self.V1 = [self.s_start]
+        self.V2 = [self.s_goal]
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(s_start, s_goal)
+        self.utils = utils.Utils()
+
+        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
+
+    def planning(self):
+        for i in range(self.iter_max):
+            node_rand = self.generate_random_node(self.s_goal, self.goal_sample_rate)
+            node_near = self.nearest_neighbor(self.V1, node_rand)
+            node_new = self.new_state(node_near, node_rand)
+
+            if node_new and not self.utils.is_collision(node_near, node_new):
+                self.V1.append(node_new)
+                node_near_prim = self.nearest_neighbor(self.V2, node_new)
+                node_new_prim = self.new_state(node_near_prim, node_new)
+
+                if node_new_prim and not self.utils.is_collision(node_new_prim, node_near_prim):
+                    self.V2.append(node_new_prim)
+
+                    while True:
+                        node_new_prim2 = self.new_state(node_new_prim, node_new)
+                        if node_new_prim2 and not self.utils.is_collision(node_new_prim2, node_new_prim):
+                            self.V2.append(node_new_prim2)
+                            node_new_prim = self.change_node(node_new_prim, node_new_prim2)
+                        else:
+                            break
+
+                        if self.is_node_same(node_new_prim, node_new):
+                            break
+
+                if self.is_node_same(node_new_prim, node_new):
+                    return self.extract_path(node_new, node_new_prim)
+
+            if len(self.V2) < len(self.V1):
+                list_mid = self.V2
+                self.V2 = self.V1
+                self.V1 = list_mid
+
+        return None
+
+    @staticmethod
+    def change_node(node_new_prim, node_new_prim2):
+        node_new = Node((node_new_prim2.x, node_new_prim2.y))
+        node_new.parent = node_new_prim
+
+        return node_new
+
+    @staticmethod
+    def is_node_same(node_new_prim, node_new):
+        if node_new_prim.x == node_new.x and \
+                node_new_prim.y == node_new.y:
+            return True
+
+        return False
+
+    def generate_random_node(self, sample_goal, goal_sample_rate):
+        delta = self.utils.delta
+
+        if np.random.random() > goal_sample_rate:
+            return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                         np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+
+        return sample_goal
+
+    @staticmethod
+    def nearest_neighbor(node_list, n):
+        return 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):
+        dist, theta = self.get_distance_and_angle(node_start, node_end)
+
+        dist = min(self.step_len, dist)
+        node_new = Node((node_start.x + dist * math.cos(theta),
+                         node_start.y + dist * math.sin(theta)))
+        node_new.parent = node_start
+
+        return node_new
+
+    @staticmethod
+    def extract_path(node_new, node_new_prim):
+        path1 = [(node_new.x, node_new.y)]
+        node_now = node_new
+
+        while node_now.parent is not None:
+            node_now = node_now.parent
+            path1.append((node_now.x, node_now.y))
+
+        path2 = [(node_new_prim.x, node_new_prim.y)]
+        node_now = node_new_prim
+
+        while node_now.parent is not None:
+            node_now = node_now.parent
+            path2.append((node_now.x, node_now.y))
+
+        return list(list(reversed(path1)) + path2)
+
+    @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)
+
+
+def main():
+    x_start = (2, 2)  # Starting node
+    x_goal = (49, 24)  # Goal node
+
+    rrt_conn = RrtConnect(x_start, x_goal, 0.8, 0.05, 5000)
+    path = rrt_conn.planning()
+
+    rrt_conn.plotting.animation_connect(rrt_conn.V1, rrt_conn.V2, path, "RRT_CONNECT")
+
+
+if __name__ == '__main__':
+    main()

Sampling_based_Planning/rrt_2D/rrt_star.py

@@ -0,0 +1,184 @@
+"""
+RRT_star 2D
+@author: damminhtien
+"""
+
+import os
+import sys
+import math
+import numpy as np
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils, queue
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
+
+
+class RrtStar:
+    def __init__(self, x_start, x_goal, step_len,
+                 goal_sample_rate, search_radius, iter_max):
+        self.s_start = Node(x_start)
+        self.s_goal = Node(x_goal)
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.search_radius = search_radius
+        self.iter_max = iter_max
+        self.vertex = [self.s_start]
+        self.path = []
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(x_start, x_goal)
+        self.utils = utils.Utils()
+
+        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
+
+    def planning(self):
+        for k in range(self.iter_max):
+            node_rand = self.generate_random_node(self.goal_sample_rate)
+            node_near = self.nearest_neighbor(self.vertex, node_rand)
+            node_new = self.new_state(node_near, node_rand)
+
+            if k % 500 == 0:
+                print(k)
+
+            if node_new and not self.utils.is_collision(node_near, node_new):
+                neighbor_index = self.find_near_neighbor(node_new)
+                self.vertex.append(node_new)
+
+                if neighbor_index:
+                    self.choose_parent(node_new, neighbor_index)
+                    self.rewire(node_new, neighbor_index)
+
+        index = self.search_goal_parent()
+        self.path = self.extract_path(self.vertex[index])
+
+        self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(self.iter_max))
+
+    def new_state(self, node_start, node_goal):
+        dist, theta = self.get_distance_and_angle(node_start, node_goal)
+
+        dist = min(self.step_len, dist)
+        node_new = Node((node_start.x + dist * math.cos(theta),
+                         node_start.y + dist * math.sin(theta)))
+
+        node_new.parent = node_start
+
+        return node_new
+
+    def choose_parent(self, node_new, neighbor_index):
+        cost = [self.get_new_cost(self.vertex[i], node_new) for i in neighbor_index]
+
+        cost_min_index = neighbor_index[int(np.argmin(cost))]
+        node_new.parent = self.vertex[cost_min_index]
+
+    def rewire(self, node_new, neighbor_index):
+        for i in neighbor_index:
+            node_neighbor = self.vertex[i]
+
+            if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor):
+                node_neighbor.parent = node_new
+
+    def search_goal_parent(self):
+        dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex]
+        node_index = [i for i in range(len(dist_list)) if dist_list[i] <= self.step_len]
+
+        if len(node_index) > 0:
+            cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index
+                         if not self.utils.is_collision(self.vertex[i], self.s_goal)]
+            return node_index[int(np.argmin(cost_list))]
+
+        return len(self.vertex) - 1
+
+    def get_new_cost(self, node_start, node_end):
+        dist, _ = self.get_distance_and_angle(node_start, node_end)
+
+        return self.cost(node_start) + dist
+
+    def generate_random_node(self, goal_sample_rate):
+        delta = self.utils.delta
+
+        if np.random.random() > goal_sample_rate:
+            return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                         np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+
+        return self.s_goal
+
+    def find_near_neighbor(self, node_new):
+        n = len(self.vertex) + 1
+        r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
+
+        dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
+        dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and
+                            not self.utils.is_collision(node_new, self.vertex[ind])]
+
+        return dist_table_index
+
+    @staticmethod
+    def nearest_neighbor(node_list, n):
+        return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+                                        for nd in node_list]))]
+
+    @staticmethod
+    def cost(node_p):
+        node = node_p
+        cost = 0.0
+
+        while node.parent:
+            cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
+            node = node.parent
+
+        return cost
+
+    def update_cost(self, parent_node):
+        OPEN = queue.QueueFIFO()
+        OPEN.put(parent_node)
+
+        while not OPEN.empty():
+            node = OPEN.get()
+
+            if len(node.child) == 0:
+                continue
+
+            for node_c in node.child:
+                node_c.Cost = self.get_new_cost(node, node_c)
+                OPEN.put(node_c)
+
+    def extract_path(self, node_end):
+        path = [[self.s_goal.x, self.s_goal.y]]
+        node = node_end
+
+        while node.parent is not None:
+            path.append([node.x, node.y])
+            node = node.parent
+        path.append([node.x, node.y])
+
+        return path
+
+    @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)
+
+
+def main():
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
+
+    rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)
+    rrt_star.planning()
+
+
+if __name__ == '__main__':
+    main()

Sampling_based_Planning/rrt_2D/rrt_star_smart.py

@@ -0,0 +1,311 @@
+"""
+RRT_STAR_SMART 2D
+@author: damminhtien
+"""
+
+import os
+import sys
+import math
+import random
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from scipy.spatial.transform import Rotation as Rot
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
+
+
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
+
+
+class RrtStarSmart:
+    def __init__(self, x_start, x_goal, step_len,
+                 goal_sample_rate, search_radius, iter_max):
+        self.x_start = Node(x_start)
+        self.x_goal = Node(x_goal)
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.search_radius = search_radius
+        self.iter_max = iter_max
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(x_start, x_goal)
+        self.utils = utils.Utils()
+
+        self.fig, self.ax = plt.subplots()
+        self.delta = self.utils.delta
+        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.V = [self.x_start]
+        self.beacons = []
+        self.beacons_radius = 2
+        self.direct_cost_old = np.inf
+        self.obs_vertex = self.utils.get_obs_vertex()
+        self.path = None
+
+    def planning(self):
+        n = 0
+        b = 2
+        InitPathFlag = False
+        self.ReformObsVertex()
+
+        for k in range(self.iter_max):
+            if k % 200 == 0:
+                print(k)
+
+            if (k - n) % b == 0 and len(self.beacons) > 0:
+                x_rand = self.Sample(self.beacons)
+            else:
+                x_rand = self.Sample()
+
+            x_nearest = self.Nearest(self.V, x_rand)
+            x_new = self.Steer(x_nearest, x_rand)
+
+            if x_new and not self.utils.is_collision(x_nearest, x_new):
+                X_near = self.Near(self.V, x_new)
+                self.V.append(x_new)
+
+                if X_near:
+                    # choose parent
+                    cost_list = [self.Cost(x_near) + self.Line(x_near, x_new) for x_near in X_near]
+                    x_new.parent = X_near[int(np.argmin(cost_list))]
+
+                    # rewire
+                    c_min = self.Cost(x_new)
+                    for x_near in X_near:
+                        c_near = self.Cost(x_near)
+                        c_new = c_min + self.Line(x_new, x_near)
+                        if c_new < c_near:
+                            x_near.parent = x_new
+
+                if not InitPathFlag and self.InitialPathFound(x_new):
+                    InitPathFlag = True
+                    n = k
+
+                if InitPathFlag:
+                    self.PathOptimization(x_new)
+                if k % 5 == 0:
+                    self.animation()
+
+        self.path = self.ExtractPath()
+        self.animation()
+        plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r')
+        plt.pause(0.01)
+        plt.show()
+
+    def PathOptimization(self, node):
+        direct_cost_new = 0.0
+        node_end = self.x_goal
+
+        while node.parent:
+            node_parent = node.parent
+            if not self.utils.is_collision(node_parent, node_end):
+                node_end.parent = node_parent
+            else:
+                direct_cost_new += self.Line(node, node_end)
+                node_end = node
+
+            node = node_parent
+
+        if direct_cost_new < self.direct_cost_old:
+            self.direct_cost_old = direct_cost_new
+            self.UpdateBeacons()
+
+    def UpdateBeacons(self):
+        node = self.x_goal
+        beacons = []
+
+        while node.parent:
+            near_vertex = [v for v in self.obs_vertex
+                           if (node.x - v[0]) ** 2 + (node.y - v[1]) ** 2 < 9]
+            if len(near_vertex) > 0:
+                for v in near_vertex:
+                    beacons.append(v)
+
+            node = node.parent
+
+        self.beacons = beacons
+
+    def ReformObsVertex(self):
+        obs_vertex = []
+
+        for obs in self.obs_vertex:
+            for vertex in obs:
+                obs_vertex.append(vertex)
+
+        self.obs_vertex = obs_vertex
+
+    def Steer(self, x_start, x_goal):
+        dist, theta = self.get_distance_and_angle(x_start, x_goal)
+        dist = min(self.step_len, dist)
+        node_new = Node((x_start.x + dist * math.cos(theta),
+                         x_start.y + dist * math.sin(theta)))
+        node_new.parent = x_start
+
+        return node_new
+
+    def Near(self, nodelist, node):
+        n = len(self.V) + 1
+        r = 50 * math.sqrt((math.log(n) / n))
+
+        dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
+        X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and
+                  not self.utils.is_collision(node, nodelist[ind])]
+
+        return X_near
+
+    def Sample(self, goal=None):
+        if goal is None:
+            delta = self.utils.delta
+            goal_sample_rate = self.goal_sample_rate
+
+            if np.random.random() > goal_sample_rate:
+                return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                             np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+
+            return self.x_goal
+        else:
+            R = self.beacons_radius
+            r = random.uniform(0, R)
+            theta = random.uniform(0, 2 * math.pi)
+            ind = random.randint(0, len(goal) - 1)
+
+            return Node((goal[ind][0] + r * math.cos(theta),
+                         goal[ind][1] + r * math.sin(theta)))
+
+    def SampleFreeSpace(self):
+        delta = self.delta
+
+        if np.random.random() > self.goal_sample_rate:
+            return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                         np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+
+        return self.x_goal
+
+    def ExtractPath(self):
+        path = []
+        node = self.x_goal
+
+        while node.parent:
+            path.append([node.x, node.y])
+            node = node.parent
+
+        path.append([self.x_start.x, self.x_start.y])
+
+        return path
+
+    def InitialPathFound(self, node):
+        if self.Line(node, self.x_goal) < self.step_len:
+            return True
+
+        return False
+
+    @staticmethod
+    def Nearest(nodelist, n):
+        return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2
+                                       for nd in nodelist]))]
+
+    @staticmethod
+    def Line(x_start, x_goal):
+        return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y)
+
+    @staticmethod
+    def Cost(node):
+        cost = 0.0
+        if node.parent is None:
+            return cost
+
+        while node.parent:
+            cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
+            node = node.parent
+
+        return cost
+
+    @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)
+
+    def animation(self):
+        plt.cla()
+        self.plot_grid("rrt*-Smart, N = " + str(self.iter_max))
+        plt.gcf().canvas.mpl_connect(
+            'key_release_event',
+            lambda event: [exit(0) if event.key == 'escape' else None])
+
+        for node in self.V:
+            if node.parent:
+                plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g")
+
+        if self.beacons:
+            theta = np.arange(0, 2 * math.pi, 0.1)
+            r = self.beacons_radius
+
+            for v in self.beacons:
+                x = v[0] + r * np.cos(theta)
+                y = v[1] + r * np.sin(theta)
+                plt.plot(x, y, linestyle='--', linewidth=2, color='darkorange')
+
+        plt.pause(0.01)
+
+    def plot_grid(self, name):
+
+        for (ox, oy, w, h) in self.obs_boundary:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3)
+        plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+
+def main():
+    x_start = (18, 8)  # Starting node
+    x_goal = (37, 18)  # Goal node
+
+    rrt = RrtStarSmart(x_start, x_goal, 1.5, 0.10, 0, 1000)
+    rrt.planning()
+
+
+if __name__ == '__main__':
+    main()