update RRT*

2026-08-29 08:34:46 +08:00 · 2020-06-23 18:46:44 -07:00
parent a22f3ef98a
commit dfee5555c5
9 changed files with 301 additions and 82 deletions
@@ -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)
@@ -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)
@@ -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 = [
@@ -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):
@@ -20,10 +20,6 @@
  </component>
  <component name="ChangeListManager">
    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/RRT.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/RRT.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/env.py" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/node.py" beforeDir="false" />
-      <change beforePath="$PROJECT_DIR$/../Sampling-based Planning/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Sampling-based Planning/plotting.py" afterDir="false" />
      <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
    </list>
    <option name="SHOW_DIALOG" value="false" />
@@ -53,7 +49,7 @@
    <property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
    <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
    <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../../PythonRobotics/PathPlanning" />
    <property name="restartRequiresConfirmation" value="false" />
    <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
  </component>
@@ -199,10 +195,11 @@
    <option name="oldMeFiltersMigrated" value="true" />
  </component>
  <component name="WindowStateProjectService">
-    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl" timestamp="1592802293738">
-      <screen x="1920" y="0" width="1920" height="1080" />
+    <state x="819" y="314" width="424" height="482" key="FileChooserDialogImpl" timestamp="1592933974409">
+      <screen x="65" y="24" width="1855" height="1056" />
    </state>
    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592802293738" />
+    <state x="819" y="314" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@65.24.1855.1056" timestamp="1592933974409" />
    <state width="1832" height="296" key="GridCell.Tab.0.bottom" timestamp="1592801972746">
      <screen x="1920" y="0" width="1920" height="1080" />
    </state>