diff --git a/Search-based Planning/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20,_12_26_AM_[Default_Changelist]/shelved.patch b/Search-based Planning/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20,_12_26_AM_[Default_Changelist]/shelved.patch
deleted file mode 100644
index bf880e7..0000000
--- a/Search-based Planning/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20,_12_26_AM_[Default_Changelist]/shelved.patch
+++ /dev/null
@@ -1,299 +0,0 @@
-Index: ara_star.py
-IDEA additional info:
-Subsystem: com.intellij.openapi.diff.impl.patch.BaseRevisionTextPatchEP
-<+>import queue\nimport plotting\nimport env\n\nimport matplotlib.pyplot as plt\n\n\nclass AraStar:\n def __init__(self, x_start, x_goal, heuristic_type):\n self.xI, self.xG = x_start, x_goal\n self.heuristic_type = heuristic_type\n\n self.Env = env.Env() # class Env\n\n self.u_set = self.Env.motions # feasible input set\n self.obs = self.Env.obs # position of obstacles\n\n self.e = 2.5\n self.g = {self.xI: 0, self.xG: float(\"inf\")}\n self.fig_name = \"ARA_Star Algorithm\"\n\n self.OPEN = queue.QueuePrior() # priority queue / OPEN\n self.CLOSED = []\n self.INCONS = []\n self.parent = {self.xI: self.xI}\n\n self.path = []\n self.visited = []\n\n def searching(self):\n self.OPEN.put(self.xI, self.fvalue(self.xI))\n self.ImprovePath()\n self.path.append(self.extract_path())\n\n while self.update_e() > 1:\n self.e -= 0.5\n print(self.e)\n OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS\n self.OPEN = queue.QueuePrior()\n self.OPEN.put(self.xI, self.fvalue(self.xI))\n\n for x in OPEN_mid:\n self.OPEN.put(x, self.fvalue(x))\n\n self.INCONS = []\n self.CLOSED = []\n self.ImprovePath()\n self.path.append(self.extract_path())\n\n return self.path, self.visited\n\n def ImprovePath(self):\n visited_each = []\n while (self.fvalue(self.xG) >\n min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):\n s = self.OPEN.get()\n\n if s not in self.CLOSED:\n self.CLOSED.append(s)\n\n for u_next in self.u_set:\n s_next = tuple([s[i] + u_next[i] for i in range(len(s))])\n\n if s_next not in self.obs:\n new_cost = self.g[s] + self.get_cost(s, u_next)\n if s_next not in self.g or new_cost < self.g[s_next]:\n self.g[s_next] = new_cost\n self.parent[s_next] = s\n visited_each.append(s_next)\n\n if s_next not in self.CLOSED:\n self.OPEN.put(s_next, self.fvalue(s_next))\n else:\n self.INCONS.append(s_next)\n\n self.visited.append(visited_each)\n\n def update_e(self):\n c_OPEN, c_INCONS = float(\"inf\"), float(\"inf\")\n\n if not self.OPEN.empty():\n c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate())\n\n if len(self.INCONS) != 0:\n c_INCONS = min(self.g[x] + self.Heuristic(x) for x in self.INCONS)\n\n if min(c_OPEN, c_INCONS) == float(\"inf\"):\n return 1\n\n return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))\n\n def fvalue(self, x):\n h = self.e * self.Heuristic(x)\n return self.g[x] + h\n\n def extract_path(self):\n \"\"\"\n Extract the path based on the relationship of nodes.\n\n :param policy: Action needed for transfer between two nodes\n :return: The planning path\n \"\"\"\n\n path_back = [self.xG]\n x_current = self.xG\n\n while True:\n x_current = self.parent[x_current]\n path_back.append(x_current)\n\n if x_current == self.xI:\n break\n\n return list(path_back)\n\n @staticmethod\n def get_cost(x, u):\n \"\"\"\n Calculate cost for this motion\n\n :param x: current node\n :param u: input\n :return: cost for this motion\n :note: cost function could be more complicate!\n \"\"\"\n\n return 1\n\n def Heuristic(self, state):\n \"\"\"\n Calculate heuristic.\n :param state: current node (state)\n :param goal: goal node (state)\n :param heuristic_type: choosing different heuristic functions\n :return: heuristic\n \"\"\"\n\n heuristic_type = self.heuristic_type\n goal = self.xG\n\n if heuristic_type == \"manhattan\":\n return abs(goal[0] - state[0]) + abs(goal[1] - state[1])\n elif heuristic_type == \"euclidean\":\n return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2)\n else:\n print(\"Please choose right heuristic type!\")\n\n\ndef main():\n x_start = (5, 5) # Starting node\n x_goal = (49, 5) # Goal node\n\n arastar = AraStar(x_start, x_goal, \"manhattan\")\n plot = plotting.Plotting(x_start, x_goal)\n\n fig_name = \"ARA* algorithm\"\n path, visited = arastar.searching()\n\n plot.animation_ara_star(path, visited, fig_name)\n\n\nif __name__ == '__main__':\n main()\n
-Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
-<+>UTF-8
-===================================================================
---- ara_star.py (revision 3a87e5d5770f7a88af23b1cf0cf579c63bb5f346)
-+++ ara_star.py (date 1593242576435)
-@@ -2,8 +2,6 @@
- import plotting
- import env
-
--import matplotlib.pyplot as plt
--
-
- class AraStar:
- def __init__(self, x_start, x_goal, heuristic_type):
-Index: .idea/workspace.xml
-IDEA additional info:
-Subsystem: com.intellij.openapi.diff.impl.patch.BaseRevisionTextPatchEP
-<+>\n\n \n \n \n \n \n \n \n
\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n
\n \n \n \n \n \n \n \n \n 1592347358698\n \n \n 1592347358698\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n
-Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
-<+>UTF-8
-===================================================================
---- .idea/workspace.xml (revision 3a87e5d5770f7a88af23b1cf0cf579c63bb5f346)
-+++ .idea/workspace.xml (date 1593242779309)
-@@ -20,9 +20,14 @@
-
-
-
-+
-+
-+
-+
-
-+
-+
-
--
-
-
-
-@@ -202,22 +207,22 @@
-
-
-
--
-+
-
-
--
--
-+
-+
-
-
--
--
-+
-+
-
-
--
--
-+
-+
-
-
--
-+
-
-
-
-Index: Astar_3D/queue.py
-IDEA additional info:
-Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
-<+>UTF-8
-===================================================================
---- Astar_3D/queue.py (date 1593242505075)
-+++ Astar_3D/queue.py (date 1593242505075)
-@@ -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
-Index: Astar_3D/Astar3D.py
-IDEA additional info:
-Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
-<+>UTF-8
-===================================================================
---- Astar_3D/Astar3D.py (date 1593242505075)
-+++ Astar_3D/Astar3D.py (date 1593242505075)
-@@ -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()
-+
-\ No newline at end of file
-Index: ../Sampling-based Planning/rrt_3D/rrtstar3D.py
-IDEA additional info:
-Subsystem: com.intellij.openapi.diff.impl.patch.BaseRevisionTextPatchEP
-<+>\"\"\"\nThis is rrt star code for 3D\n@author: yue qi\n\"\"\"\nimport numpy as np\nfrom numpy.matlib import repmat\nfrom collections import defaultdict\nimport time\nimport matplotlib.pyplot as plt\n\nimport os\nimport sys\n\nsys.path.append(os.path.dirname(os.path.abspath(__file__)) + \"/../../Sampling-based Planning/\")\nfrom rrt_3D.env3D import env\nfrom rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, hash3D, dehash\n\n\nclass rrtstar():\n def __init__(self):\n self.env = env()\n self.Parent = {}\n self.E = edgeset()\n self.V = []\n self.i = 0\n self.maxiter = 4000 # at least 4000 in this env\n self.stepsize = 0.5\n self.gamma = 500\n self.eta = 1.1*self.stepsize\n self.Path = []\n self.done = False\n\n def wireup(self,x,y):\n self.E.add_edge([x,y]) # add edge\n self.Parent[hash3D(x)] = y\n\n def removewire(self,xnear):\n xparent = self.Parent[hash3D(xnear)]\n a = [xnear,xparent]\n self.E.remove_edge(a) # remove and replace old the connection\n\n def reached(self):\n self.done = True\n xn = near(self,self.env.goal)\n c = [cost(self,x) for x in xn]\n xncmin = xn[np.argmin(c)]\n self.wireup(self.env.goal,xncmin)\n self.V.append(self.env.goal)\n self.Path,self.D = path(self)\n\n def run(self):\n self.V.append(self.env.start)\n self.ind = 0\n xnew = self.env.start\n print('start rrt*... ')\n self.fig = plt.figure(figsize = (10,8))\n while self.ind < self.maxiter:\n xrand = sampleFree(self)\n xnearest = nearest(self,xrand)\n xnew = steer(self,xnearest,xrand)\n if not isCollide(self,xnearest,xnew):\n Xnear = near(self,xnew)\n self.V.append(xnew) # add point\n visualization(self)\n # minimal path and minimal cost\n xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)\n # connecting along minimal cost path\n for xnear in Xnear:\n c1 = cost(self, xnear) + getDist(xnew, xnear)\n if not isCollide(self, xnew, xnear) and c1 < cmin:\n xmin, cmin = xnear, c1\n self.wireup(xnew, xmin)\n # rewire\n for xnear in Xnear:\n c2 = cost(self, xnew) + getDist(xnew, xnear)\n if not isCollide(self, xnew, xnear) and c2 < cost(self, xnear):\n self.removewire(xnear)\n self.wireup(xnear, xnew)\n self.i += 1\n self.ind += 1\n # max sample reached\n self.reached()\n print('time used = ' + str(time.time()-starttime))\n print('Total distance = '+str(self.D))\n visualization(self)\n plt.show()\n \n\nif __name__ == '__main__':\n p = rrtstar()\n starttime = time.time()\n p.run()\n \n
-Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
-<+>UTF-8
-===================================================================
---- ../Sampling-based Planning/rrt_3D/rrtstar3D.py (revision 3a87e5d5770f7a88af23b1cf0cf579c63bb5f346)
-+++ ../Sampling-based Planning/rrt_3D/rrtstar3D.py (date 1593242505075)
-@@ -23,7 +23,7 @@
- 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 @@
- 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
-Index: Astar_3D/utils3D.py
-IDEA additional info:
-Subsystem: com.intellij.openapi.diff.impl.patch.CharsetEP
-<+>UTF-8
-===================================================================
---- Astar_3D/utils3D.py (date 1593242505075)
-+++ Astar_3D/utils3D.py (date 1593242505075)
-@@ -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)])
-\ No newline at end of file
-diff --git 3D/env3D.py Astar_3D/env3D.py
-rename from 3D/env3D.py
-rename to Astar_3D/env3D.py
-diff --git 3D/plot_util3D.py Astar_3D/plot_util3D.py
-rename from 3D/plot_util3D.py
-rename to Astar_3D/plot_util3D.py
diff --git a/Search-based Planning/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml b/Search-based Planning/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml
deleted file mode 100644
index c5122ea..0000000
--- a/Search-based Planning/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 7873bbb..b1f00a4 100644
--- a/Search-based Planning/.idea/workspace.xml
+++ b/Search-based Planning/.idea/workspace.xml
@@ -20,7 +20,31 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -50,7 +74,7 @@
-
+
@@ -72,7 +96,7 @@
-
+
@@ -136,6 +160,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -157,27 +202,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -201,19 +225,19 @@
-
diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search-based Planning/Search_2D/ARAstar.py
index 68e11bf..58e3c12 100644
--- a/Search-based Planning/Search_2D/ARAstar.py
+++ b/Search-based Planning/Search_2D/ARAstar.py
@@ -153,8 +153,7 @@ class AraStar:
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
- @staticmethod
- def cost(s_start, s_goal):
+ def cost(self, s_start, s_goal):
"""
Calculate cost for this motion
:param s_start: starting node
@@ -163,7 +162,27 @@ class AraStar:
:note: cost function could be more complicate!
"""
- return 1
+ if self.is_collision(s_start, s_goal):
+ return float("inf")
+
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
def main():
diff --git a/Search-based Planning/Search_2D/D_star.py b/Search-based Planning/Search_2D/D_star.py
index 9b09dc0..cd220d3 100644
--- a/Search-based Planning/Search_2D/D_star.py
+++ b/Search-based Planning/Search_2D/D_star.py
@@ -5,6 +5,7 @@ D_star 2D
import os
import sys
+import math
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
@@ -15,11 +16,11 @@ from Search_2D import env
class Dstar:
- def __init__(self, x_start, x_goal):
- self.xI, self.xG = x_start, x_goal
+ def __init__(self, s_start, s_goal):
+ self.s_start, self.s_goal = s_start, s_goal
self.Env = env.Env()
- self.Plot = plotting.Plotting(self.xI, self.xG)
+ self.Plot = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions
self.obs = self.Env.obs
@@ -30,16 +31,21 @@ class Dstar:
self.OPEN = set()
self.t = {}
self.PARENT = {}
- self.h = {self.xG: 0}
+ self.h = {}
self.k = {}
self.path = []
+ self.visited = []
+ self.count = 0
for i in range(self.Env.x_range):
for j in range(self.Env.y_range):
self.t[(i, j)] = 'NEW'
- self.k[(i, j)] = 0
+ self.k[(i, j)] = 0.0
+ self.h[(i, j)] = float("inf")
self.PARENT[(i, j)] = None
+ self.h[self.s_goal] = 0.0
+
def run(self, s_start, s_end):
self.insert(s_end, 0)
while True:
@@ -61,28 +67,31 @@ class Dstar:
print("Add obstacle at: x =", x, ",", "y =", y)
self.obs.add((x, y))
plt.plot(x, y, 'sk')
- if (x, y) in self.path:
- s = self.xI
- while s != self.xG:
- if self.PARENT[s] in self.obs:
- self.modify(s)
- continue
- s = self.PARENT[s]
- self.path = self.extract_path(self.xI, self.xG)
+ s = self.s_start
+ self.visited = []
+ while s != self.s_goal:
+ if self.is_collision(s, self.PARENT[s]):
+ self.modify(s)
+ continue
+ s = self.PARENT[s]
+ self.path = self.extract_path(self.s_start, self.s_goal)
+ self.plot_visited(self.visited)
self.plot_path(self.path)
+ self.count += 1
self.fig.canvas.draw_idle()
def extract_path(self, s_start, s_end):
- path = []
+ path = [s_start]
s = s_start
while True:
s = self.PARENT[s]
+ path.append(s)
if s == s_end:
return path
- path.append(s)
def process_state(self):
s = self.min_state()
+ self.visited.append(s)
if s is None:
return -1
k_old = self.get_k_min()
@@ -156,6 +165,7 @@ class Dstar:
def get_neighbor(self, s):
nei_list = set()
+
for u in self.u_set:
s_next = tuple([s[i] + u[i] for i in range(2)])
if s_next not in self.obs:
@@ -163,16 +173,55 @@ class Dstar:
return nei_list
- def cost(self, s_start, s_end):
- if s_start in self.obs or s_end in self.obs:
- return float("inf")
- return 1
+ def cost(self, s_start, s_goal):
+ """
+ Calculate cost for this motion
+ :param s_start: starting node
+ :param s_goal: end node
+ :return: cost for this motion
+ :note: cost function could be more complicate!
+ """
- @staticmethod
- def plot_path(path):
+ if self.is_collision(s_start, s_goal):
+ return float("inf")
+
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
+
+ def plot_path(self, path):
px = [x[0] for x in path]
py = [x[1] for x in path]
- plt.plot(px, py, marker='o')
+ plt.plot(px, py, linewidth=2)
+ plt.plot(self.s_start[0], self.s_start[1], "bs")
+ plt.plot(self.s_goal[0], self.s_goal[1], "gs")
+
+ def plot_visited(self, visited):
+ color = ['gainsboro', 'lightgray', 'silver', 'darkgray',
+ 'bisque', 'navajowhite', 'moccasin', 'wheat',
+ 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']
+
+ if self.count >= len(color) - 1:
+ self.count = 0
+
+ for x in visited:
+ if x not in self.obs:
+ plt.plot(x[0], x[1], marker='s', color=color[self.count])
def main():
diff --git a/Search-based Planning/Search_2D/FieldD_star.py b/Search-based Planning/Search_2D/FieldD_star.py
new file mode 100644
index 0000000..e69de29
diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py
index 8553347..0b8479f 100644
--- a/Search-based Planning/Search_2D/LPAstar.py
+++ b/Search-based Planning/Search_2D/LPAstar.py
@@ -28,8 +28,7 @@ class LpaStar:
self.x = self.Env.x_range
self.y = self.Env.y_range
- self.U = {}
- self.g, self.rhs = {}, {}
+ self.g, self.rhs, self.U = {}, {}, {}
for i in range(self.Env.x_range):
for j in range(self.Env.y_range):
@@ -38,13 +37,15 @@ class LpaStar:
self.rhs[self.s_start] = 0
self.U[self.s_start] = self.CalculateKey(self.s_start)
+ self.visited = []
+ self.count = 0
self.fig = plt.figure()
def run(self):
self.Plot.plot_grid("Lifelong Planning A*")
- self.ComputePath()
+ self.ComputeShortestPath()
self.plot_path(self.extract_path())
self.fig.canvas.mpl_connect('button_press_event', self.on_press)
@@ -57,6 +58,8 @@ class LpaStar:
else:
x, y = int(x), int(y)
print("Change position: x =", x, ",", "y =", y)
+ self.visited = []
+ self.count += 1
if (x, y) not in self.obs:
self.obs.add((x, y))
plt.plot(x, y, 'sk')
@@ -68,13 +71,15 @@ class LpaStar:
for s_n in self.get_neighbor((x, y)):
self.UpdateVertex(s_n)
- self.ComputePath()
+ self.ComputeShortestPath()
+ self.plot_visited(self.visited)
self.plot_path(self.extract_path())
self.fig.canvas.draw_idle()
- def ComputePath(self):
+ def ComputeShortestPath(self):
while True:
s, v = self.TopKey()
+ self.visited.append(s)
if v >= self.CalculateKey(self.s_goal) and \
self.rhs[self.s_goal] == self.g[self.s_goal]:
break
@@ -141,19 +146,36 @@ class LpaStar:
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
- def cost(self, s_start, s_end):
+ def cost(self, s_start, s_goal):
"""
- calculate edge cost: (s_start, s_end)
- :param s_start: start node
- :param s_end: end node
- :return: cost
+ Calculate cost for this motion
+ :param s_start: starting node
+ :param s_goal: end node
+ :return: cost for this motion
+ :note: cost function could be more complicate!
"""
- # if one of the vertex in obstacles: return infinity.
- if s_start in self.obs or s_end in self.obs:
+ if self.is_collision(s_start, s_goal):
return float("inf")
- return 1
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
def extract_path(self):
"""
@@ -161,32 +183,46 @@ class LpaStar:
:return: The planning path
"""
- path = []
+ path = [self.s_goal]
s = self.s_goal
for k in range(100):
g_list = {}
for x in self.get_neighbor(s):
- g_list[x] = self.g[x]
+ if not self.is_collision(s, x):
+ g_list[x] = self.g[x]
s = min(g_list, key=g_list.get)
+ path.append(s)
if s == self.s_start:
break
- path.append(s)
return list(reversed(path))
- @staticmethod
- def plot_path(path):
+ def plot_path(self, path):
px = [x[0] for x in path]
py = [x[1] for x in path]
- plt.plot(px, py, marker='o')
+ plt.plot(px, py, linewidth=2)
+ plt.plot(self.s_start[0], self.s_start[1], "bs")
+ plt.plot(self.s_goal[0], self.s_goal[1], "gs")
+
+ def plot_visited(self, visited):
+ color = ['gainsboro', 'lightgray', 'silver', 'darkgray',
+ 'bisque', 'navajowhite', 'moccasin', 'wheat',
+ 'powderblue', 'skyblue', 'lightskyblue', 'cornflowerblue']
+
+ if self.count >= len(color) - 1:
+ self.count = 0
+
+ for x in visited:
+ if x not in self.obs:
+ plt.plot(x[0], x[1], marker='s', color=color[self.count])
def main():
x_start = (5, 5)
x_goal = (45, 25)
- lpastar = LpaStar(x_start, x_goal, "manhattan")
+ lpastar = LpaStar(x_start, x_goal, "Euclidean")
lpastar.run()
diff --git a/Search-based Planning/Search_2D/LRTAstar.py b/Search-based Planning/Search_2D/LRTAstar.py
index 34bcd1e..36b6ef0 100644
--- a/Search-based Planning/Search_2D/LRTAstar.py
+++ b/Search-based Planning/Search_2D/LRTAstar.py
@@ -132,12 +132,12 @@ class LrtAstarN:
:return: neighbors
"""
- s_list = set()
+ s_list = []
for u in self.u_set:
s_next = tuple([s[i] + u[i] for i in range(2)])
if s_next not in self.obs:
- s_list.add(s_next)
+ s_list.append(s_next)
return s_list
@@ -175,16 +175,36 @@ class LrtAstarN:
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
- def cost(self, s_start, s_end):
+ def cost(self, s_start, s_goal):
"""
Calculate cost for this motion
:param s_start: starting node
- :param s_end: end node
+ :param s_goal: end node
:return: cost for this motion
:note: cost function could be more complicate!
"""
- return 1
+ if self.is_collision(s_start, s_goal):
+ return float("inf")
+
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
def main():
diff --git a/Search-based Planning/Search_2D/RTAAstar.py b/Search-based Planning/Search_2D/RTAAstar.py
index 97b2267..1f4797b 100644
--- a/Search-based Planning/Search_2D/RTAAstar.py
+++ b/Search-based Planning/Search_2D/RTAAstar.py
@@ -184,23 +184,43 @@ class RtaAstar:
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
- def cost(self, s_start, s_end):
+ def cost(self, s_start, s_goal):
"""
Calculate cost for this motion
:param s_start: starting node
- :param s_end: end node
+ :param s_goal: end node
:return: cost for this motion
:note: cost function could be more complicate!
"""
- return 1
+ if self.is_collision(s_start, s_goal):
+ return float("inf")
+
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
def main():
s_start = (10, 5)
s_goal = (45, 25)
- rtaa = RtaAstar(s_start, s_goal, 220, "euclidean")
+ rtaa = RtaAstar(s_start, s_goal, 240, "euclidean")
plot = plotting.Plotting(s_start, s_goal)
rtaa.searching()
diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc
index 0561250..a4c2421 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
index 33df9b2..bff683c 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/astar.py b/Search-based Planning/Search_2D/astar.py
index 5bda0f8..8d2361c 100644
--- a/Search-based Planning/Search_2D/astar.py
+++ b/Search-based Planning/Search_2D/astar.py
@@ -113,15 +113,44 @@ class Astar:
:return: neighbors
"""
- s_list = set()
+ s_list = []
for u in self.u_set:
- s_next = tuple([s[i] + u[i] for i in range(2)])
- if s_next not in self.obs:
- s_list.add(s_next)
+ s_list.append(tuple([s[i] + u[i] for i in range(2)]))
return s_list
+ def cost(self, s_start, s_goal):
+ """
+ Calculate cost for this motion
+ :param s_start: starting node
+ :param s_goal: end node
+ :return: cost for this motion
+ :note: cost function could be more complicate!
+ """
+
+ if self.is_collision(s_start, s_goal):
+ return float("inf")
+
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
+
def fvalue(self, x):
"""
f = g + h. (g: cost to come, h: heuristic function)
@@ -164,18 +193,6 @@ class Astar:
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
- @staticmethod
- def cost(s_start, s_goal):
- """
- Calculate cost for this motion
- :param s_start: starting node
- :param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
- """
-
- return 1
-
def main():
s_start = (5, 5)
diff --git a/Search-based Planning/Search_2D/bfs.py b/Search-based Planning/Search_2D/bfs.py
index a8031a2..0dd7e2e 100644
--- a/Search-based Planning/Search_2D/bfs.py
+++ b/Search-based Planning/Search_2D/bfs.py
@@ -56,15 +56,32 @@ class BFS:
:return: neighbors
"""
- s_list = set()
+ s_list = []
for u in self.u_set:
s_next = tuple([s[i] + u[i] for i in range(2)])
- if s_next not in self.obs:
- s_list.add(s_next)
+ if not self.is_collision(s, s_next):
+ s_list.append(s_next)
return s_list
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
+
def extract_path(self):
"""
Extract the path based on the PARENT set.
diff --git a/Search-based Planning/Search_2D/bidirectional_a_star.py b/Search-based Planning/Search_2D/bidirectional_a_star.py
index 4f8035f..30a0437 100644
--- a/Search-based Planning/Search_2D/bidirectional_a_star.py
+++ b/Search-based Planning/Search_2D/bidirectional_a_star.py
@@ -140,8 +140,7 @@ class BidirectionalAstar:
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
- @staticmethod
- def cost(s_start, s_goal):
+ def cost(self, s_start, s_goal):
"""
Calculate cost for this motion
:param s_start: starting node
@@ -150,7 +149,27 @@ class BidirectionalAstar:
:note: cost function could be more complicate!
"""
- return 1
+ if self.is_collision(s_start, s_goal):
+ return float("inf")
+
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
def main():
diff --git a/Search-based Planning/Search_2D/dfs.py b/Search-based Planning/Search_2D/dfs.py
index c168be1..6e45d72 100644
--- a/Search-based Planning/Search_2D/dfs.py
+++ b/Search-based Planning/Search_2D/dfs.py
@@ -60,11 +60,28 @@ class DFS:
for u in self.u_set:
s_next = tuple([s[i] + u[i] for i in range(2)])
- if s_next not in self.obs:
+ if not self.is_collision(s, s_next):
s_list.append(s_next)
return s_list
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
+
def extract_path(self):
"""
Extract the path based on the relationship of nodes.
diff --git a/Search-based Planning/Search_2D/dijkstra.py b/Search-based Planning/Search_2D/dijkstra.py
index e8667de..98a2e3c 100644
--- a/Search-based Planning/Search_2D/dijkstra.py
+++ b/Search-based Planning/Search_2D/dijkstra.py
@@ -5,6 +5,7 @@ Dijkstra 2D
import os
import sys
+import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search-based Planning/")
@@ -36,13 +37,13 @@ class Dijkstra:
:return: path, order of visited nodes in the planning
"""
- while self.OPEN:
+ while not self.OPEN.empty():
s = self.OPEN.get()
-
- if s == self.s_goal: # stop condition
- break
self.CLOSED.append(s)
+ if s == self.s_goal:
+ break
+
for s_n in self.get_neighbor(s):
new_cost = self.g[s] + self.cost(s, s_n)
if s_n not in self.g:
@@ -61,12 +62,10 @@ class Dijkstra:
:return: neighbors
"""
- s_list = set()
+ s_list = []
for u in self.u_set:
- s_next = tuple([s[i] + u[i] for i in range(2)])
- if s_next not in self.obs:
- s_list.add(s_next)
+ s_list.append(tuple([s[i] + u[i] for i in range(2)]))
return s_list
@@ -88,8 +87,7 @@ class Dijkstra:
return list(path)
- @staticmethod
- def cost(s_start, s_goal):
+ def cost(self, s_start, s_goal):
"""
Calculate cost for this motion
:param s_start: starting node
@@ -98,7 +96,27 @@ class Dijkstra:
:note: cost function could be more complicate!
"""
- return 1
+ if self.is_collision(s_start, s_goal):
+ return float("inf")
+
+ return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])
+
+ def is_collision(self, s_start, s_end):
+ if s_start in self.obs or s_end in self.obs:
+ return True
+
+ if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
+ if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
+ s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ else:
+ s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
+ s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
+
+ if s1 in self.obs or s2 in self.obs:
+ return True
+
+ return False
def main():
diff --git a/Search-based Planning/Search_2D/env.py b/Search-based Planning/Search_2D/env.py
index e08da2f..0cd2424 100644
--- a/Search-based Planning/Search_2D/env.py
+++ b/Search-based Planning/Search_2D/env.py
@@ -8,7 +8,8 @@ class Env:
def __init__(self):
self.x_range = 51 # size of background
self.y_range = 31
- self.motions = [(1, 0), (-1, 0), (0, 1), (0, -1)]
+ self.motions = [(-1, 0), (-1, 1), (0, 1), (1, 1),
+ (1, 0), (1, -1), (0, -1), (-1, -1)]
self.obs = self.obs_map()
def obs_map(self):
diff --git a/Search-based Planning/Search_2D/plotting.py b/Search-based Planning/Search_2D/plotting.py
index d95da9c..c818a64 100644
--- a/Search-based Planning/Search_2D/plotting.py
+++ b/Search-based Planning/Search_2D/plotting.py
@@ -79,35 +79,34 @@ class Plotting:
for x in visited:
count += 1
- plt.plot(x[0], x[1], linewidth='3', color=cl, marker='o')
+ plt.plot(x[0], x[1], color=cl, 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
+ length = 20
elif count < len(visited) * 2 / 3:
- length = 25
- else:
length = 30
+ else:
+ length = 40
+ #
+ # length = 15
if count % length == 0:
plt.pause(0.001)
plt.pause(0.01)
def plot_path(self, path, cl='r', flag=False):
- if self.xI in path:
- path.remove(self.xI)
-
- if self.xG in path:
- 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))]
if not flag:
- plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
+ plt.plot(path_x, path_y, linewidth='3', color='r')
else:
- plt.plot(path_x, path_y, linewidth='3', color=cl, marker='o')
+ plt.plot(path_x, path_y, linewidth='3', color=cl)
+
+ plt.plot(self.xI[0], self.xI[1], "bs")
+ plt.plot(self.xG[0], self.xG[1], "gs")
plt.pause(0.01)
diff --git a/Search-based Planning/gif/ARA_star.gif b/Search-based Planning/gif/ARA_star.gif
index dbe5aca..16a2058 100644
Binary files a/Search-based Planning/gif/ARA_star.gif and b/Search-based Planning/gif/ARA_star.gif differ
diff --git a/Search-based Planning/gif/Astar.gif b/Search-based Planning/gif/Astar.gif
index ee699c6..e14ca8b 100644
Binary files a/Search-based Planning/gif/Astar.gif and b/Search-based Planning/gif/Astar.gif differ
diff --git a/Search-based Planning/gif/BFS.gif b/Search-based Planning/gif/BFS.gif
index 1f885f5..b591db7 100644
Binary files a/Search-based Planning/gif/BFS.gif and b/Search-based Planning/gif/BFS.gif differ
diff --git a/Search-based Planning/gif/Bi-Astar.gif b/Search-based Planning/gif/Bi-Astar.gif
index 93defaf..23c7f19 100644
Binary files a/Search-based Planning/gif/Bi-Astar.gif and b/Search-based Planning/gif/Bi-Astar.gif differ
diff --git a/Search-based Planning/gif/DFS.gif b/Search-based Planning/gif/DFS.gif
index 3fe684d..a622849 100644
Binary files a/Search-based Planning/gif/DFS.gif and b/Search-based Planning/gif/DFS.gif differ
diff --git a/Search-based Planning/gif/D_star.gif b/Search-based Planning/gif/D_star.gif
new file mode 100644
index 0000000..9db826a
Binary files /dev/null and b/Search-based Planning/gif/D_star.gif differ
diff --git a/Search-based Planning/gif/Dijkstra.gif b/Search-based Planning/gif/Dijkstra.gif
index d0901eb..11a9bb4 100644
Binary files a/Search-based Planning/gif/Dijkstra.gif and b/Search-based Planning/gif/Dijkstra.gif differ
diff --git a/Search-based Planning/gif/LPAstar.gif b/Search-based Planning/gif/LPAstar.gif
new file mode 100644
index 0000000..af79a47
Binary files /dev/null and b/Search-based Planning/gif/LPAstar.gif differ
diff --git a/Search-based Planning/gif/LRTA_star.gif b/Search-based Planning/gif/LRTA_star.gif
index 6c38489..281ce1f 100644
Binary files a/Search-based Planning/gif/LRTA_star.gif and b/Search-based Planning/gif/LRTA_star.gif differ
diff --git a/Search-based Planning/gif/RTAA_star.gif b/Search-based Planning/gif/RTAA_star.gif
index 4c531b3..f58f829 100644
Binary files a/Search-based Planning/gif/RTAA_star.gif and b/Search-based Planning/gif/RTAA_star.gif differ
diff --git a/Search-based Planning/gif/RepeatedA_star.gif b/Search-based Planning/gif/RepeatedA_star.gif
index 7fdd49c..f105803 100644
Binary files a/Search-based Planning/gif/RepeatedA_star.gif and b/Search-based Planning/gif/RepeatedA_star.gif differ