update

Sampling-based Planning/.idea/.gitignore

@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml

Sampling-based Planning/.idea/Sampling-based Planning.iml

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+  <component name="TestRunnerService">
+    <option name="PROJECT_TEST_RUNNER" value="pytest" />
+  </component>
+</module>

Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

Sampling-based Planning/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7 (Search-based Planning)" project-jdk-type="Python SDK" />
+</project>

Sampling-based Planning/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/Sampling-based Planning.iml" filepath="$PROJECT_DIR$/.idea/Sampling-based Planning.iml" />
+    </modules>
+  </component>
+</project>

Sampling-based Planning/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

Sampling-based Planning/RRT.py

@@ -0,0 +1,49 @@
+import env
+import plotting
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+
+class RRT:
+    def __init__(self, xI, xG):
+        # Plotting = plotting.Plotting(xI, xG)
+        # Plotting.animation([xI, xG], [xI, xG], "zhou")
+        fig, ax = plt.subplots()
+
+        plt.axis([-5, 5, -5, 5])
+
+        ax.plot()
+
+        ax.add_patch(
+            patches.Rectangle(
+                (1, 1),
+                0.5,
+                0.5,
+                edgecolor='black',
+                facecolor='black',
+                fill=True
+            ))
+
+        ax.add_patch(
+            patches.Circle(
+                (3, 3),
+                0.5,
+                edgecolor='black',
+                facecolor='black',
+                fill=True
+            )
+        )
+
+        plt.axis("equal")
+        plt.show()
+
+
+    def planning(self):
+        return
+
+
+if __name__ == '__main__':
+    x_Start = (5, 5)  # Starting node
+    x_Goal = (49, 5)  # Goal node
+
+    rrt = RRT(x_Start, x_Goal)

Sampling-based Planning/env.py

@@ -0,0 +1,30 @@
+import numpy as np
+
+class Env:
+    def __init__(self):
+        self.x_range = (0, 50)  # size of background
+        self.y_range = (0, 30)
+        self.obs = self.obs_map()
+
+    def obs_map(self):
+        """
+        Initialize obstacles' positions
+
+        :return: map of obstacles
+        """
+
+        x = self.x_range
+        y = self.y_range
+        w = 2
+
+        obs_boundary = []
+
+        for i in np.linspace(x[0], x[1], (x[1]-x[0])//w+1):
+            obs_boundary.append((i, y[0], w))
+        for i in np.linspace(x[0], x[1], (x[1]-x[0])//w+1):
+            obs_boundary.append((i, y[1], w))
+        for j in np.linspace(y[0], y[1], (y[1]-y[0])//w+1):
+            obs_boundary.append((j, x[0], w))
+        for j in np.linspace(y[0], y[1], (y[1]-y[0])//w+1):
+            obs_boundary.append((j, x[1], w))
+

Sampling-based Planning/plotting.py

@@ -0,0 +1,53 @@
+import matplotlib.pyplot as plt
+import env
+
+
+class Plotting:
+    def __init__(self, xI, xG):
+        self.xI, self.xG = xI, xG
+        self.env = env.Env()
+        self.obs = self.env.obs_map()
+
+    def animation(self, path, visited, name):
+        self.plot_grid(name)
+        self.plot_visited(visited)
+        self.plot_path(path)
+
+    def plot_grid(self, name):
+        obs_x = [self.obs[i][0] for i in range(len(self.obs))]
+        obs_y = [self.obs[i][1] for i in range(len(self.obs))]
+
+        plt.plot(self.xI[0], self.xI[1], "bs")
+        plt.plot(self.xG[0], self.xG[1], "gs")
+        plt.plot(obs_x, obs_y, "sk")
+        plt.title(name)
+        plt.axis("equal")
+
+    def plot_visited(self, visited):
+        visited.remove(self.xI)
+        count = 0
+
+        for x in visited:
+            count += 1
+            plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
+            plt.gcf().canvas.mpl_connect('key_release_event', lambda event:
+            [exit(0) if event.key == 'escape' else None])
+
+            if count < len(visited) / 3:
+                length = 15
+            elif count < len(visited) * 2 / 3:
+                length = 30
+            else:
+                length = 45
+
+            if count % length == 0: plt.pause(0.001)
+
+    def plot_path(self, path):
+        path.remove(self.xI)
+        path.remove(self.xG)
+        path_x = [path[i][0] for i in range(len(path))]
+        path_y = [path[i][1] for i in range(len(path))]
+
+        plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
+        plt.pause(0.01)
+        plt.show()

Search-based Planning/.idea/workspace.xml

@@ -1,7 +1,31 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
+  <component name="BranchesTreeState">
+    <expand>
+      <path>
+        <item name="ROOT" type="e8cecc67:BranchNodeDescriptor" />
+        <item name="LOCAL_ROOT" type="e8cecc67:BranchNodeDescriptor" />
+      </path>
+      <path>
+        <item name="ROOT" type="e8cecc67:BranchNodeDescriptor" />
+        <item name="REMOTE_ROOT" type="e8cecc67:BranchNodeDescriptor" />
+      </path>
+      <path>
+        <item name="ROOT" type="e8cecc67:BranchNodeDescriptor" />
+        <item name="REMOTE_ROOT" type="e8cecc67:BranchNodeDescriptor" />
+        <item name="GROUP_NODE:origin" type="e8cecc67:BranchNodeDescriptor" />
+      </path>
+    </expand>
+    <select />
+  </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/.idea/.gitignore" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/.idea/misc.xml" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/.idea/vcs.xml" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/RRT.py" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/../Sampling-based Planning/env.py" afterDir="false" />
+      <change 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" />
@@ -177,22 +201,26 @@
     <option name="oldMeFiltersMigrated" value="true" />
   </component>
   <component name="WindowStateProjectService">
-    <state width="1832" height="296" key="GridCell.Tab.0.bottom" timestamp="1592801510790">
+    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl" timestamp="1592802293738">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801510790" />
+    <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 width="1832" height="296" key="GridCell.Tab.0.center" timestamp="1592801510790">
+    <state width="1832" height="296" key="GridCell.Tab.0.bottom" timestamp="1592801972746">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801510790" />
+    <state width="1832" height="296" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801972746" />
-    <state width="1832" height="296" key="GridCell.Tab.0.left" timestamp="1592801510789">
+    <state width="1832" height="296" key="GridCell.Tab.0.center" timestamp="1592801972746">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801510789" />
+    <state width="1832" height="296" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801972746" />
-    <state width="1832" height="296" key="GridCell.Tab.0.right" timestamp="1592801510790">
+    <state width="1832" height="296" key="GridCell.Tab.0.left" timestamp="1592801972746">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="296" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801510790" />
+    <state width="1832" height="296" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801972746" />
+    <state width="1832" height="296" key="GridCell.Tab.0.right" timestamp="1592801972746">
+      <screen x="1920" y="0" width="1920" height="1080" />
+    </state>
+    <state width="1832" height="296" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801972746" />
     <state x="2406" y="174" key="SettingsEditor" timestamp="1592801555194">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>