Merge remote-tracking branch 'origin/master'

Sampling-based Planning/rrt_3D/rrtstar3D.py

@@ -23,7 +23,7 @@ class rrtstar():
         self.E = edgeset()
         self.V = []
         self.i = 0
-        self.maxiter = 4000 # at least 4000 in this env
+        self.maxiter = 10000 # at least 4000 in this env
         self.stepsize = 0.5
         self.gamma = 500
         self.eta = 1.1*self.stepsize
@@ -61,7 +61,7 @@ class rrtstar():
             if not isCollide(self,xnearest,xnew):
                 Xnear = near(self,xnew)
                 self.V.append(xnew) # add point
-                visualization(self)
+                # visualization(self)
                 # minimal path and minimal cost
                 xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
                 # connecting along minimal cost path

Search-based Planning/Astar_3D/Astar3D.py

@@ -0,0 +1,38 @@
+# this is the three dimensional A* algo
+# !/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: yue qi
+"""
+import numpy as np
+
+import os
+import sys
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+from Astar_3D.env3D import env
+from Astar_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest
+import queue
+
+
+class Weighted_A_star(object):
+    def __init__(self):
+        self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
+                      [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
+                      [1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
+                      [1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
+        self.env = env()
+        self.Space = StateSpace(self.env.boundary) # key is the point, store g value
+        self.OPEN = queue.QueuePrior() # store [point,priority]
+        self.start = getNearest(self.Space,self.env.start)
+        self.goal = getNearest(self.Space,self.env.goal)
+        self.h = Heuristic(self.Space,self.goal)
+        self.Parent = {}
+        self.CLOSED = {}
+        
+
+    def run(self):
+        pass
+if __name__ == '__main__':
+    Astar = Weighted_A_star()
+    

Search-based Planning/Astar_3D/queue.py

@@ -0,0 +1,62 @@
+import collections
+import heapq
+
+
+class QueueFIFO:
+    """
+    Class: QueueFIFO
+    Description: QueueFIFO is designed for First-in-First-out rule.
+    """
+
+    def __init__(self):
+        self.queue = collections.deque()
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, node):
+        self.queue.append(node)  # enter from back
+
+    def get(self):
+        return self.queue.popleft()  # leave from front
+
+
+class QueueLIFO:
+    """
+    Class: QueueLIFO
+    Description: QueueLIFO is designed for Last-in-First-out rule.
+    """
+
+    def __init__(self):
+        self.queue = collections.deque()
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, node):
+        self.queue.append(node)  # enter from back
+
+    def get(self):
+        return self.queue.pop()  # leave from back
+
+
+class QueuePrior:
+    """
+    Class: QueuePrior
+    Description: QueuePrior reorders elements using value [priority]
+    """
+
+    def __init__(self):
+        self.queue = []
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, item, priority):
+        heapq.heappush(self.queue, (priority, item))  # reorder x using priority
+
+    def get(self):
+        return heapq.heappop(self.queue)[1]  # pop out the smallest item
+
+    def enumerate(self):
+        return self.queue

Search-based Planning/Astar_3D/utils3D.py

@@ -0,0 +1,66 @@
+import numpy as np
+
+def getRay(x, y):
+    direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
+    return np.array([x, direc])
+
+def getAABB(blocks):
+    AABB = []
+    for i in blocks:
+        AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
+    return AABB
+
+def getDist(pos1, pos2):
+    return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
+
+def getNearest(Space,pt):
+    '''get the nearest point on the grid'''
+    mindis,minpt = 1000,None
+    for strpts in Space.keys(): 
+        pts = dehash(strpts)
+        dis = getDist(pts,pt)
+        if dis < mindis:
+            mindis,minpt = dis,pts
+    return minpt
+
+def Heuristic(Space,t):
+    '''Max norm distance'''
+    h = {}
+    for k in Space.keys():
+        h[k] = max(abs(t-dehash(k)))
+    return h
+
+def hash3D(x):
+    return str(x[0])+' '+str(x[1])+' '+str(x[2])
+
+def dehash(x):
+    return np.array([float(i) for i in x.split(' ')])
+
+def isinbound(i, x):
+    if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
+        return True
+    return False
+
+def StateSpace(boundary,factor=0):
+    '''This function is used to get nodes and discretize the space.
+       State space is by x*y*z,3 where each 3 is a point in 3D.'''
+    xmin,xmax = boundary[0]+factor,boundary[3]-factor
+    ymin,ymax = boundary[1]+factor,boundary[4]-factor
+    zmin,zmax = boundary[2]+factor,boundary[5]-factor
+    xarr = np.arange(xmin,xmax,1)
+    yarr = np.arange(ymin,ymax,1)
+    zarr = np.arange(zmin,zmax,1)
+    V = np.meshgrid(xarr,yarr,zarr)
+    VV = np.reshape(V,[3,len(xarr)*len(yarr)*len(zarr)]) # all points in 3D
+    Space = {}
+    for v in VV.T:
+        Space[hash3D(v)] = 0 # this hashmap initialize all g values at 0
+    return Space
+
+if __name__ == "__main__":
+    from env3D import env
+    env = env(resolution=1)
+    Space = StateSpace(env.boundary,0)
+    t = np.array([3.0,4.0,5.0])
+    h = Heuristic(Space,t)
+    print(h[hash3D(t)])