update planning

Planning/GraphSearch/Deterministic/Dstar.py

@@ -88,7 +88,7 @@ def main(self):
         self.OPEN.put(self.s_start, self.f_value(self.s_start))
         path, node_list = self.compute_shortest_path()
         self.plot = plotting.Plotting(self.s_start, self.s_goal)
-        self.plot.animation(node_list, path, "A*", show=False)
+        self.plot.animation(node_list, path, "D*", show=False)
         self.fig = plt.gcf()
         self.fig.canvas.mpl_connect('button_press_event', self.on_press)
         plt.show()

Planning/GraphSearch/Deterministic/anytime_Dstar.py

@@ -1,5 +1,5 @@
 """
-D* 
+Anytime D* 
 """
 
 import os
@@ -92,7 +92,7 @@ def main(self):
         self.OPEN.put(self.s_start, self.f_value(self.s_start))
         path, node_list = self.compute_shortest_path()
         self.plot = plotting.Plotting(self.s_start, self.s_goal)
-        self.plot.animation(node_list, path, "A*", show=False)
+        self.plot.animation(node_list, path, "Anytime D*", show=False)
         self.fig = plt.gcf()
         self.fig.canvas.mpl_connect('button_press_event', self.on_press)
         plt.show()
@@ -210,9 +210,10 @@ def on_press(self, event):
             old, new = self.split_old_new_nodes()
             plt.cla()
             self.plot.plot_visited(new, False, c='-b')
-            self.plot.animation(old, path, "D*", show=False)
+            self.plot.animation(old, path, "Anytime D*", show=False)
 
             self.fig.canvas.draw_idle()
+
     def split_old_new_nodes(self):
         all_nodes = self.trans_node_list(self.PARENT.keys())
         old = []

Planning/GraphSearch/Probabilistic/batch_informed_trees.py → Planning/GraphSearch/Stochastic/batch_informed_trees.py

@@ -379,7 +379,7 @@ def draw_ellipse(x_center, c_best, dist, theta):
         t = np.arange(0, 2 * math.pi + 0.1, 0.2)
         x = [a * math.cos(it) for it in t]
         y = [b * math.sin(it) for it in t]
-        rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2]
+        rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]
         fx = rot @ np.array([x, y])
         px = np.array(fx[0, :] + cx).flatten()
         py = np.array(fx[1, :] + cy).flatten()

Planning/GraphSearch/Probabilistic/rrt.py → Planning/GraphSearch/Stochastic/rrt.py

@@ -14,7 +14,11 @@
 import env, plotting, utils
 
 
-
+class Node:
+    def __init__(self, n):
+        self.x = n[0]
+        self.y = n[1]
+        self.parent = None
 class RRT:
     def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max):
         self.s_start = Node(s_start)
@@ -97,7 +101,7 @@ def main():
     x_start = (2, 2)  # Starting node
     x_goal = (49, 24)  # Goal node
 
-    rrt = RRT(x_start, x_goal, 0.5, 0.05, 10000)
+    rrt = RRT(x_start, x_goal, 0.5, 0.1, 10000)
     path = rrt.planning()
 
     if path:

Planning/GraphSearch/Probabilistic/rrt_star.py → Planning/GraphSearch/Stochastic/rrt_star.py

@@ -8,7 +8,7 @@
 import sys
 import math
 import numpy as np
-
+import matplotlib.pyplot as plt
 sys.path.append(os.path.dirname(os.path.abspath(__file__))+'/../')
 
 import env, plotting, utils, queue
@@ -49,9 +49,10 @@ def planning(self):
             node_near = self.nearest_neighbor(self.vertex, node_rand)
             node_new = self.new_state(node_near, node_rand)
 
-            if k % 500 == 0:
+            if k % 100 == 0:
                 print(k)
-
+                self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(k),show=False)
+                plt.pause(0.5)
             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)
@@ -61,8 +62,8 @@ def planning(self):
 
         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))
+        self.plotting.animation(self.vertex, self.path, "rrt*, N = " + str(self.iter_max),show=True)
+
 
     def new_state(self, node_start, node_goal):
         dist, theta = self.get_distance_and_angle(node_start, node_goal)
@@ -161,7 +162,7 @@ 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, 2000)
+    rrt_star = RRTStar(x_start, x_goal, 5, 0.10, 20, 2000)
     rrt_star.planning()
 
 

Planning/README.md

@@ -1,8 +1,44 @@
 Overview
 ----------
 
+Deterministic Search
+--------------------
 
-Spline Interpolation
+<table>
+<tbody>
+  <tr>
+    <td><img src="figure/Dijkstra.png" alt="dijkstra"></td>
+    <td><img src="figure/astar.png" alt="A*"></td>
+  </tr>
+    <tr>
+    <td><img src="figure/dstar.gif" alt="D*"></td>
+    <td><img src="figure/anytime_dstar.gif" alt="Anytime D*"></td>
+  </tr>
+</tbody>
+</table>
+
+Stochastic Search
+--------------------
+
+<table>
+<tbody>
+  <tr>
+    <td><img src="figure/rrt.gif" alt="RRT"></td>
+    <td><img src="figure/rrt_connect.gif" alt="RRT Connect"></td>
+  </tr>
+  <tr>
+    <td><img src="figure/rrtstar.gif" alt="RRT*"></td>
+    <td><img src="figure/dynamic_rrt.gif" alt="Dynamic RRT"></td>
+  </tr>
+    <tr>
+    <td><img src="figure/bitstar.gif" alt="BIT*"></td>
+
+  </tr>
+</tbody>
+</table>
+
+
+Spline Curve
 ----------
 
 <table>
@@ -14,17 +50,17 @@ Spline Interpolation
 </tbody>
 </table>
 
-Polynomail Trajectory
----------------------
-
+<!-- Polynomail Trajectory
+--------------------- -->
+<!--
 <table>
 <tbody>
   <tr>
     <td><img src="figure/cubic_polynomial.gif" alt="cubic_polynomial"></td>
     <td><img src="figure/quintic_polynomial.gif" alt="quintic_polynomial"></td>
   </tr>
 </tbody>
-</table>
+</table> -->
 
 Dubins/Reeds Shepp Curve
 ----------