'IRRTstar'

.vscode/.ropeproject/config.py

@@ -0,0 +1,114 @@
+# The default ``config.py``
+# flake8: noqa
+
+
+def set_prefs(prefs):
+    """This function is called before opening the project"""
+
+    # Specify which files and folders to ignore in the project.
+    # Changes to ignored resources are not added to the history and
+    # VCSs.  Also they are not returned in `Project.get_files()`.
+    # Note that ``?`` and ``*`` match all characters but slashes.
+    # '*.pyc': matches 'test.pyc' and 'pkg/test.pyc'
+    # 'mod*.pyc': matches 'test/mod1.pyc' but not 'mod/1.pyc'
+    # '.svn': matches 'pkg/.svn' and all of its children
+    # 'build/*.o': matches 'build/lib.o' but not 'build/sub/lib.o'
+    # 'build//*.o': matches 'build/lib.o' and 'build/sub/lib.o'
+    prefs['ignored_resources'] = ['*.pyc', '*~', '.ropeproject',
+                                  '.hg', '.svn', '_svn', '.git', '.tox']
+
+    # Specifies which files should be considered python files.  It is
+    # useful when you have scripts inside your project.  Only files
+    # ending with ``.py`` are considered to be python files by
+    # default.
+    # prefs['python_files'] = ['*.py']
+
+    # Custom source folders:  By default rope searches the project
+    # for finding source folders (folders that should be searched
+    # for finding modules).  You can add paths to that list.  Note
+    # that rope guesses project source folders correctly most of the
+    # time; use this if you have any problems.
+    # The folders should be relative to project root and use '/' for
+    # separating folders regardless of the platform rope is running on.
+    # 'src/my_source_folder' for instance.
+    # prefs.add('source_folders', 'src')
+
+    # You can extend python path for looking up modules
+    # prefs.add('python_path', '~/python/')
+
+    # Should rope save object information or not.
+    prefs['save_objectdb'] = True
+    prefs['compress_objectdb'] = False
+
+    # If `True`, rope analyzes each module when it is being saved.
+    prefs['automatic_soa'] = True
+    # The depth of calls to follow in static object analysis
+    prefs['soa_followed_calls'] = 0
+
+    # If `False` when running modules or unit tests "dynamic object
+    # analysis" is turned off.  This makes them much faster.
+    prefs['perform_doa'] = True
+
+    # Rope can check the validity of its object DB when running.
+    prefs['validate_objectdb'] = True
+
+    # How many undos to hold?
+    prefs['max_history_items'] = 32
+
+    # Shows whether to save history across sessions.
+    prefs['save_history'] = True
+    prefs['compress_history'] = False
+
+    # Set the number spaces used for indenting.  According to
+    # :PEP:`8`, it is best to use 4 spaces.  Since most of rope's
+    # unit-tests use 4 spaces it is more reliable, too.
+    prefs['indent_size'] = 4
+
+    # Builtin and c-extension modules that are allowed to be imported
+    # and inspected by rope.
+    prefs['extension_modules'] = []
+
+    # Add all standard c-extensions to extension_modules list.
+    prefs['import_dynload_stdmods'] = True
+
+    # If `True` modules with syntax errors are considered to be empty.
+    # The default value is `False`; When `False` syntax errors raise
+    # `rope.base.exceptions.ModuleSyntaxError` exception.
+    prefs['ignore_syntax_errors'] = False
+
+    # If `True`, rope ignores unresolvable imports.  Otherwise, they
+    # appear in the importing namespace.
+    prefs['ignore_bad_imports'] = False
+
+    # If `True`, rope will insert new module imports as
+    # `from <package> import <module>` by default.
+    prefs['prefer_module_from_imports'] = False
+
+    # If `True`, rope will transform a comma list of imports into
+    # multiple separate import statements when organizing
+    # imports.
+    prefs['split_imports'] = False
+
+    # If `True`, rope will remove all top-level import statements and
+    # reinsert them at the top of the module when making changes.
+    prefs['pull_imports_to_top'] = True
+
+    # If `True`, rope will sort imports alphabetically by module name instead
+    # of alphabetically by import statement, with from imports after normal
+    # imports.
+    prefs['sort_imports_alphabetically'] = False
+
+    # Location of implementation of
+    # rope.base.oi.type_hinting.interfaces.ITypeHintingFactory In general
+    # case, you don't have to change this value, unless you're an rope expert.
+    # Change this value to inject you own implementations of interfaces
+    # listed in module rope.base.oi.type_hinting.providers.interfaces
+    # For example, you can add you own providers for Django Models, or disable
+    # the search type-hinting in a class hierarchy, etc.
+    prefs['type_hinting_factory'] = (
+        'rope.base.oi.type_hinting.factory.default_type_hinting_factory')
+
+
+def project_opened(project):
+    """This function is called after opening the project"""
+    # Do whatever you like here!

Sampling_based_Planning/rrt_3D/env3D.py

@@ -93,6 +93,7 @@ class env():
         # self.OBB = []
         self.start = np.array([0.5, 2.5, 5.5])
         self.goal = np.array([19.0, 2.5, 5.5])
+        # self.goal = np.array([5.0, 2.5, 5.5])
         self.t = 0 # time 
 
     def New_block(self):

Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py

@@ -17,27 +17,50 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/")
 from rrt_3D.env3D import env
-from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, isinside, near, nearest
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, isinside, near, nearest, path
 from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 from rrt_3D.queue import MinheapPQ
 
+
+def CreateUnitSphere(r = 1):
+    phi = np.linspace(0,2*np.pi, 256).reshape(256, 1) # the angle of the projection in the xy-plane
+    theta = np.linspace(0, np.pi, 256).reshape(-1, 256) # the angle from the polar axis, ie the polar angle
+    radius = r
+
+    # Transformation formulae for a spherical coordinate system.
+    x = radius*np.sin(theta)*np.cos(phi)
+    y = radius*np.sin(theta)*np.sin(phi)
+    z = radius*np.cos(theta)
+    return (x, y, z)
+
+def draw_ellipsoid(ax, C, L, xcenter):
+    (xs, ys, zs) = CreateUnitSphere()
+    pts = np.array([xs, ys, zs])
+    pts_in_world_frame = C@L@pts + xcenter
+    ax.plot_surface(pts_in_world_frame[0], pts_in_world_frame[1], pts_in_world_frame[2], alpha=0.05, color="g")
+
 class IRRT:
 
-    def __init__(self):
+    def __init__(self,  show_ellipse = False):
         self.env = env()
         self.xstart, self.xgoal = tuple(self.env.start), tuple(self.env.goal)
         self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
         self.Parent = {}
-        self.N = 10000 # used for determining how many batches needed
+        self.Path = []
+        self.N = 5000 # used for determining how many batches needed
         self.ind = 0
         self.i = 0
-        # rrt* near 
+        # rrt* near and other utils
         self.stepsize = 0.5
         self.gamma = 500
         self.eta = self.stepsize
         self.rgoal = self.stepsize
-
         self.done = False
+        # for drawing the ellipse
+        self.C = np.zeros([3,3])
+        self.L = np.zeros([3,3])
+        self.xcenter = np.zeros(3)
+        self.show_ellipse = show_ellipse
 
     def Informed_rrt(self):
         self.V = [self.xstart]
@@ -48,6 +71,7 @@ class IRRT:
         c = 1
         while self.ind <= self.N:
             print(self.ind)
+            self.visualization()
             # print(self.i)
             if len(self.Xsoln) == 0:
                 cbest = np.inf
@@ -89,13 +113,18 @@ class IRRT:
                 self.i += 1
                 if self.InGoalRegion(xnew):
                     print('reached')
+                    self.done = True
+                    self.Parent[self.xgoal] = xnew
+                    self.Path, _ = path(self)
                     self.Xsoln.add(xnew)
-                
+            # update path
+            if self.done:
+                self.Path, _ = path(self, Path = [])
             self.ind += 1
         # return tree
         return self.T
                 
-    def Sample(self, xstart, xgoal, cmax):
+    def Sample(self, xstart, xgoal, cmax, bias = 0.05):
         # sample within a eclipse 
         if cmax < np.inf:
             cmin = getDist(xgoal, xstart)
@@ -108,12 +137,15 @@ class IRRT:
             L = np.diag(r) # R3*3 
             xball = self.SampleUnitBall() # np.array
             x =  C@L@xball + xcenter
+            self.C = C # save to global var
+            self.xcenter = xcenter
+            self.L = L
             if not isinside(self, x): # intersection with the state space
                 xrand = x
             else:
                 return self.Sample(xstart, xgoal, cmax)
         else:
-            xrand = sampleFree(self, bias = 0.0)
+            xrand = sampleFree(self, bias = bias)
         return xrand
 
     def SampleUnitBall(self):
@@ -153,14 +185,53 @@ class IRRT:
     def line(self, x, y):
         return getDist(x, y)
 
-    def g_hat(self, x):
-        # heuristic estimate from start to x
-        return getDist(x, self.xstart)
-
-    def h_hat(self, x):
-        # heuristic estimate from x to goal
-        return getDist(x, self.xgoal)
+    def visualization(self):
+        if self.ind % 100 == 0:
+            V = np.array(self.V)
+            edges = list(map(list, self.E))
+            Path = np.array(self.Path)
+            start = self.env.start
+            goal = self.env.goal
+            # edges = E.get_edge()
+            #----------- list structure
+            # edges = []
+            # for i in self.Parent:
+            #     edges.append([i,self.Parent[i]])
+            #----------- end
+            # generate axis objects
+            ax = plt.subplot(111, projection='3d')
+            
+            # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi))
+            # ax.view_init(elev=0., azim=90.)
+            ax.view_init(elev=8., azim=90.)
+            # ax.view_init(elev=-8., azim=180)
+            ax.clear()
+            # drawing objects
+            draw_Spheres(ax, self.env.balls)
+            draw_block_list(ax, self.env.blocks)
+            if self.env.OBB is not None:
+                draw_obb(ax, self.env.OBB)
+            draw_block_list(ax, np.array([self.env.boundary]), alpha=0)
+            draw_line(ax, edges, visibility=0.75, color='g')
+            draw_line(ax, Path, color='r')
+            if self.show_ellipse:
+                draw_ellipsoid(ax, self.C, self.L, self.xcenter) # beware, depending on start and goal position, this might be bad for vis
+            if len(V) > 0:
+                ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', )
+            ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
+            ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
+            # adjust the aspect ratio
+            xmin, xmax = self.env.boundary[0], self.env.boundary[3]
+            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
+            zmin, zmax = self.env.boundary[2], self.env.boundary[5]
+            dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
+            ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
+            make_transparent(ax)
+            #plt.xlabel('s')
+            #plt.ylabel('y')
+            ax.set_axis_off()
+            plt.pause(0.0001)
 
 if __name__ == '__main__':
-    A = IRRT()
+    A = IRRT(show_ellipse=False)
     A.Informed_rrt()