pathplanning 0.1.0__py3-none-any.whl

Sampling_based_Planning/rrt_2D/dubins_rrt_star.py

@@ -0,0 +1,321 @@
+"""
+DUBINS_RRT_STAR 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
+import CurvesGenerator.dubins_path as dubins
+import CurvesGenerator.draw as draw
+
+
+class Node:
+    def __init__(self, x, y, yaw):
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.parent = None
+        self.cost = 0.0
+        self.path_x = []
+        self.path_y = []
+        self.paty_yaw = []
+
+
+class DubinsRRTStar:
+    def __init__(self, sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len,
+                 goal_sample_rate, search_radius, iter_max):
+        self.s_start = Node(sx, sy, syaw)
+        self.s_goal = Node(gx, gy, gyaw)
+        self.vr = vehicle_radius
+        self.step_len = step_len
+        self.goal_sample_rate = goal_sample_rate
+        self.search_radius = search_radius
+        self.iter_max = iter_max
+        self.curv = 1
+
+        self.env = env.Env()
+        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.obs_circle()
+        self.obs_boundary = self.env.obs_boundary
+        self.utils.update_obs(self.obs_circle, self.obs_boundary, [])
+
+        self.V = [self.s_start]
+        self.path = None
+
+    def planning(self):
+
+        for i in range(self.iter_max):
+            print("Iter:", i, ", number of nodes:", len(self.V))
+            rnd = self.Sample()
+            node_nearest = self.Nearest(self.V, rnd)
+            new_node = self.Steer(node_nearest, rnd)
+
+            if new_node and not self.is_collision(new_node):
+                near_indexes = self.Near(self.V, new_node)
+                new_node = self.choose_parent(new_node, near_indexes)
+
+                if new_node:
+                    self.V.append(new_node)
+                    self.rewire(new_node, near_indexes)
+
+            if i % 5 == 0:
+                self.draw_graph()
+
+        last_index = self.search_best_goal_node()
+
+        path = self.generate_final_course(last_index)
+        print("get!")
+        px = [s[0] for s in path]
+        py = [s[1] for s in path]
+        plt.plot(px, py, '-r')
+        plt.pause(0.01)
+        plt.show()
+
+    def draw_graph(self, rnd=None):
+        plt.cla()
+        # for stopping simulation with the esc key.
+        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.path_x, node.path_y, "-g")
+
+        self.plot_grid("dubins rrt*")
+        plt.plot(self.s_start.x, self.s_start.y, "xr")
+        plt.plot(self.s_goal.x, self.s_goal.y, "xr")
+        plt.grid(True)
+        self.plot_start_goal_arrow()
+        plt.pause(0.01)
+
+    def plot_start_goal_arrow(self):
+        draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2, "darkorange")
+        draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2, "darkorange")
+
+    def generate_final_course(self, goal_index):
+        print("final")
+        path = [[self.s_goal.x, self.s_goal.y]]
+        node = self.V[goal_index]
+        while node.parent:
+            for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
+                path.append([ix, iy])
+            node = node.parent
+        path.append([self.s_start.x, self.s_start.y])
+        return path
+
+    def calc_dist_to_goal(self, x, y):
+        dx = x - self.s_goal.x
+        dy = y - self.s_goal.y
+        return math.hypot(dx, dy)
+
+    def search_best_goal_node(self):
+        dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.V]
+        goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.step_len]
+
+        safe_goal_inds = []
+        for goal_ind in goal_inds:
+            t_node = self.Steer(self.V[goal_ind], self.s_goal)
+            if t_node and not self.is_collision(t_node):
+                safe_goal_inds.append(goal_ind)
+
+        if not safe_goal_inds:
+            return None
+
+        min_cost = min([self.V[i].cost for i in safe_goal_inds])
+        for i in safe_goal_inds:
+            if self.V[i].cost == min_cost:
+                return i
+
+        return None
+
+    def rewire(self, new_node, near_inds):
+        for i in near_inds:
+            near_node = self.V[i]
+            edge_node = self.Steer(new_node, near_node)
+            if not edge_node:
+                continue
+            edge_node.cost = self.calc_new_cost(new_node, near_node)
+
+            no_collision = ~self.is_collision(edge_node)
+            improved_cost = near_node.cost > edge_node.cost
+
+            if no_collision and improved_cost:
+                self.V[i] = edge_node
+                self.propagate_cost_to_leaves(new_node)
+
+    def choose_parent(self, new_node, near_inds):
+        if not near_inds:
+            return None
+
+        costs = []
+        for i in near_inds:
+            near_node = self.V[i]
+            t_node = self.Steer(near_node, new_node)
+            if t_node and not self.is_collision(t_node):
+                costs.append(self.calc_new_cost(near_node, new_node))
+            else:
+                costs.append(float("inf"))  # the cost of collision node
+        min_cost = min(costs)
+
+        if min_cost == float("inf"):
+            print("There is no good path.(min_cost is inf)")
+            return None
+
+        min_ind = near_inds[costs.index(min_cost)]
+        new_node = self.Steer(self.V[min_ind], new_node)
+
+        return new_node
+
+    def calc_new_cost(self, from_node, to_node):
+        d, _ = self.get_distance_and_angle(from_node, to_node)
+        return from_node.cost + d
+
+    def propagate_cost_to_leaves(self, parent_node):
+        for node in self.V:
+            if node.parent == parent_node:
+                node.cost = self.calc_new_cost(parent_node, node)
+                self.propagate_cost_to_leaves(node)
+
+    @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 Near(self, nodelist, node):
+        n = len(nodelist) + 1
+        r = min(self.search_radius * math.sqrt((math.log(n)) / n), self.step_len)
+
+        dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist]
+        node_near_ind = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2]
+
+        return node_near_ind
+
+    def Steer(self, node_start, node_end):
+        sx, sy, syaw = node_start.x, node_start.y, node_start.yaw
+        gx, gy, gyaw = node_end.x, node_end.y, node_end.yaw
+        maxc = self.curv
+
+        path = dubins.calc_dubins_path(sx, sy, syaw, gx, gy, gyaw, maxc)
+
+        if len(path.x) <= 1:
+            return None
+
+        node_new = Node(path.x[-1], path.y[-1], path.yaw[-1])
+        node_new.path_x = path.x
+        node_new.path_y = path.y
+        node_new.path_yaw = path.yaw
+        node_new.cost = node_start.cost + path.L
+        node_new.parent = node_start
+
+        return node_new
+
+    def Sample(self):
+        delta = self.utils.delta
+
+        if random.random() > self.goal_sample_rate:
+            return Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+                        random.uniform(self.y_range[0] + delta, self.y_range[1] - delta),
+                        random.uniform(-math.pi, math.pi))
+        else:
+            return self.s_goal
+
+    @staticmethod
+    def Nearest(nodelist, n):
+        return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2
+                                       for nd in nodelist]))]
+
+    def is_collision(self, node):
+        for ox, oy, r in self.obs_circle:
+            dx = [ox - x for x in node.path_x]
+            dy = [oy - y for y in node.path_y]
+            dist = np.hypot(dx, dy)
+
+            if min(dist) < r + self.delta:
+                return True
+
+        return False
+
+    def animation(self):
+        self.plot_grid("dubins rrt*")
+        self.plot_arrow()
+        plt.show()
+
+    def plot_arrow(self):
+        draw.Arrow(self.s_start.x, self.s_start.y, self.s_start.yaw, 2.5, "darkorange")
+        draw.Arrow(self.s_goal.x, self.s_goal.y, self.s_goal.yaw, 2.5, "darkorange")
+
+    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, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.s_start.x, self.s_start.y, "bs", linewidth=3)
+        plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+    @staticmethod
+    def obs_circle():
+        obs_cir = [
+            [10, 10, 3],
+            [15, 22, 3],
+            [22, 8, 2.5],
+            [26, 16, 2],
+            [37, 10, 3],
+            [37, 23, 3],
+            [45, 15, 2]
+        ]
+
+        return obs_cir
+
+
+def main():
+    sx, sy, syaw = 5, 5, np.deg2rad(90)
+    gx, gy, gyaw = 45, 25, np.deg2rad(0)
+    goal_sample_rate = 0.1
+    search_radius = 50.0
+    step_len = 30.0
+    iter_max = 250
+    vehicle_radius = 2.0
+
+    drrtstar = DubinsRRTStar(sx, sy, syaw, gx, gy, gyaw, vehicle_radius, step_len,
+                             goal_sample_rate, search_radius, iter_max)
+    drrtstar.planning()
+
+
+if __name__ == '__main__':
+    main()

Sampling_based_Planning/rrt_2D/dynamic_rrt.py

@@ -0,0 +1,342 @@
+"""
+DYNAMIC_RRT_2D
+@author: damminhtien
+"""
+
+import os
+import sys
+import math
+import copy
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+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
+        self.flag = "VALID"
+
+
+class Edge:
+    def __init__(self, n_p, n_c):
+        self.parent = n_p
+        self.child = n_c
+        self.flag = "VALID"
+
+
+class DynamicRrt:
+    def __init__(self, s_start, s_goal, step_len, goal_sample_rate, waypoint_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.waypoint_sample_rate = waypoint_sample_rate
+        self.iter_max = iter_max
+        self.vertex = [self.s_start]
+        self.vertex_old = []
+        self.vertex_new = []
+        self.edges = []
+
+        self.env = env.Env()
+        self.plotting = plotting.Plotting(s_start, s_goal)
+        self.utils = utils.Utils()
+        self.fig, self.ax = plt.subplots()
+
+        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.obs_add = [0, 0, 0]
+
+        self.path = []
+        self.waypoint = []
+
+    def planning(self):
+        for i 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 node_new and not self.utils.is_collision(node_near, node_new):
+                self.vertex.append(node_new)
+                self.edges.append(Edge(node_near, node_new))
+                dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
+
+                if dist <= self.step_len:
+                    self.new_state(node_new, self.s_goal)
+
+                    path = self.extract_path(node_new)
+                    self.plot_grid("Dynamic_RRT")
+                    self.plot_visited()
+                    self.plot_path(path)
+                    self.path = path
+                    self.waypoint = self.extract_waypoint(node_new)
+                    self.fig.canvas.mpl_connect('button_press_event', self.on_press)
+                    plt.show()
+
+                    return
+
+        return None
+
+    def on_press(self, event):
+        x, y = event.xdata, event.ydata
+        if x < 0 or x > 50 or y < 0 or y > 30:
+            print("Please choose right area!")
+        else:
+            x, y = int(x), int(y)
+            print("Add circle obstacle at: s =", x, ",", "y =", y)
+            self.obs_add = [x, y, 2]
+            self.obs_circle.append([x, y, 2])
+            self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle)
+            self.InvalidateNodes()
+
+            if self.is_path_invalid():
+                print("Path is Replanning ...")
+                path, waypoint = self.replanning()
+
+                print("len_vertex: ", len(self.vertex))
+                print("len_vertex_old: ", len(self.vertex_old))
+                print("len_vertex_new: ", len(self.vertex_new))
+
+                plt.cla()
+                self.plot_grid("Dynamic_RRT")
+                self.plot_vertex_old()
+                self.plot_path(self.path, color='blue')
+                self.plot_vertex_new()
+                self.vertex_new = []
+                self.plot_path(path)
+                self.path = path
+                self.waypoint = waypoint
+            else:
+                print("Trimming Invalid Nodes ...")
+                self.TrimRRT()
+
+                plt.cla()
+                self.plot_grid("Dynamic_RRT")
+                self.plot_visited(animation=False)
+                self.plot_path(self.path)
+
+            self.fig.canvas.draw_idle()
+
+    def InvalidateNodes(self):
+        for edge in self.edges:
+            if self.is_collision_obs_add(edge.parent, edge.child):
+                edge.child.flag = "INVALID"
+
+    def is_path_invalid(self):
+        for node in self.waypoint:
+            if node.flag == "INVALID":
+                return True
+
+    def is_collision_obs_add(self, start, end):
+        delta = self.utils.delta
+        obs_add = self.obs_add
+
+        if math.hypot(start.x - obs_add[0], start.y - obs_add[1]) <= obs_add[2] + delta:
+            return True
+
+        if math.hypot(end.x - obs_add[0], end.y - obs_add[1]) <= obs_add[2] + delta:
+            return True
+
+        o, d = self.utils.get_ray(start, end)
+        if self.utils.is_intersect_circle(o, d, [obs_add[0], obs_add[1]], obs_add[2]):
+            return True
+
+        return False
+
+    def replanning(self):
+        self.TrimRRT()
+
+        for i in range(self.iter_max):
+            node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate)
+            node_near = self.nearest_neighbor(self.vertex, 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.vertex.append(node_new)
+                self.vertex_new.append(node_new)
+                self.edges.append(Edge(node_near, node_new))
+                dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
+
+                if dist <= self.step_len:
+                    self.new_state(node_new, self.s_goal)
+                    path = self.extract_path(node_new)
+                    waypoint = self.extract_waypoint(node_new)
+                    print("path: ", len(path))
+                    print("waypoint: ", len(waypoint))
+
+                    return path, waypoint
+
+        return None
+
+    def TrimRRT(self):
+        for i in range(1, len(self.vertex)):
+            node = self.vertex[i]
+            node_p = node.parent
+            if node_p.flag == "INVALID":
+                node.flag = "INVALID"
+
+        self.vertex = [node for node in self.vertex if node.flag == "VALID"]
+        self.vertex_old = copy.deepcopy(self.vertex)
+        self.edges = [Edge(node.parent, node) for node in self.vertex[1:len(self.vertex)]]
+
+    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 generate_random_node_replanning(self, goal_sample_rate, waypoint_sample_rate):
+        delta = self.utils.delta
+        p = np.random.random()
+
+        if p < goal_sample_rate:
+            return self.s_goal
+        elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate:
+            return self.waypoint[np.random.randint(0, len(self.waypoint) - 1)]
+        else:
+            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)))
+
+    @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
+
+    def extract_path(self, node_end):
+        path = [(self.s_goal.x, self.s_goal.y)]
+        node_now = node_end
+
+        while node_now.parent is not None:
+            node_now = node_now.parent
+            path.append((node_now.x, node_now.y))
+
+        return path
+
+    def extract_waypoint(self, node_end):
+        waypoint = [self.s_goal]
+        node_now = node_end
+
+        while node_now.parent is not None:
+            node_now = node_now.parent
+            waypoint.append(node_now)
+
+        return waypoint
+
+    @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 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.s_start.x, self.s_start.y, "bs", linewidth=3)
+        plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+    def plot_visited(self, animation=True):
+        if animation:
+            count = 0
+            for node in self.vertex:
+                count += 1
+                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])
+                    if count % 10 == 0:
+                        plt.pause(0.001)
+        else:
+            for node in self.vertex:
+                if node.parent:
+                    plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+
+    def plot_vertex_old(self):
+        for node in self.vertex_old:
+            if node.parent:
+                plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+
+    def plot_vertex_new(self):
+        count = 0
+
+        for node in self.vertex_new:
+            count += 1
+            if node.parent:
+                plt.plot([node.parent.x, node.x], [node.parent.y, node.y], color='darkorange')
+                plt.gcf().canvas.mpl_connect('key_release_event',
+                                             lambda event:
+                                             [exit(0) if event.key == 'escape' else None])
+                if count % 10 == 0:
+                    plt.pause(0.001)
+
+    @staticmethod
+    def plot_path(path, color='red'):
+        plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color)
+        plt.pause(0.01)
+
+
+def main():
+    x_start = (2, 2)  # Starting node
+    x_goal = (49, 24)  # Goal node
+
+    drrt = DynamicRrt(x_start, x_goal, 0.5, 0.1, 0.6, 5000)
+    drrt.planning()
+
+
+if __name__ == '__main__':
+    main()

Sampling_based_Planning/rrt_2D/env.py

@@ -0,0 +1,45 @@
+"""
+Environment for rrt_2D
+@author: damminhtien
+"""
+
+
+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]
+        ]
+        return obs_boundary
+
+    @staticmethod
+    def obs_rectangle():
+        obs_rectangle = [
+            [14, 12, 8, 2],
+            [18, 22, 8, 3],
+            [26, 7, 2, 12],
+            [32, 14, 10, 2]
+        ]
+        return obs_rectangle
+
+    @staticmethod
+    def obs_circle():
+        obs_cir = [
+            [7, 12, 3],
+            [46, 20, 2],
+            [15, 5, 2],
+            [37, 7, 3],
+            [37, 23, 3]
+        ]
+
+        return obs_cir