saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 16:40:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:zhm-real/path-planning-algorithms

Browse Source
This commit is contained in:
yue qi
2020-06-28 11:11:36 -07:00
parent 52c8def653 bdf75fce33
commit f807cb652a
4 changed files with 129 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+12 -7
View File
@@ -19,7 +19,12 @@
<select />
</component>
<component name="ChangeListManager">
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
<change afterPath="$PROJECT_DIR$/Search_2D/LRTA_star.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/ara_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARA_star.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/ida_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/IDA_star.py" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -64,8 +69,8 @@
</list>
</option>
</component>
<component name="RunManager" selected="Python.bidirectionalAstar3D">
<configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
<component name="RunManager" selected="Python.LRTA_star">
<configuration name="LRTA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
@@ -77,7 +82,7 @@
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/a_star.py" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LRTA_star.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
@@ -193,19 +198,19 @@
</configuration>
<list>
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.a_star" />
<item itemvalue="Python.bidirectional_a_star" />
<item itemvalue="Python.bfs" />
<item itemvalue="Python.dfs" />
<item itemvalue="Python.bidirectionalAstar3D" />
<item itemvalue="Python.LRTA_star" />
</list>
<recent_temporary>
<list>
<item itemvalue="Python.LRTA_star" />
<item itemvalue="Python.bidirectional_a_star" />
<item itemvalue="Python.bidirectionalAstar3D" />
<item itemvalue="Python.bfs" />
<item itemvalue="Python.dfs" />
<item itemvalue="Python.bidirectional_a_star" />
<item itemvalue="Python.a_star" />
</list>
</recent_temporary>
</component>
Search-based Planning/Search_2D/ara_star.py → Search-based Planning/Search_2D/ARA_star.py
View File
Search-based Planning/Search_2D/ida_star.py → Search-based Planning/Search_2D/IDA_star.py
View File
Search-based Planning/Search_2D/LRTA_star.py
+117
View File
@@ -0,0 +1,117 @@
"""
LRTA_star 2D
@author: huiming zhou
"""
import os
import sys
import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search-based Planning/")
from Search_2D import queue
from Search_2D import plotting
from Search_2D import env
class LrtAstar:
def __init__(self, x_start, x_goal, heuristic_type):
self.xI, self.xG = x_start, x_goal
self.heuristic_type = heuristic_type
self.Env = env.Env() # class Env
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.g = {self.xI: 0, self.xG: float("inf")}
self.OPEN = queue.QueuePrior() # priority queue / OPEN
self.OPEN.put(self.xI, self.h(self.xI))
self.CLOSED = set()
self.Parent = {self.xI: self.xI}
def searching(self):
h = {self.xI: self.h(self.xI)}
s = self.xI
parent = {self.xI: self.xI}
visited = []
count = 0
while s != self.xG:
count += 1
print(count)
visited.append(s)
h_list = {}
for u in self.u_set:
s_next = tuple([s[i] + u[i] for i in range(len(s))])
if s_next not in self.obs:
if s_next not in h:
h[s_next] = self.h(s_next)
h_list[s_next] = self.get_cost(s, s_next) + h[s_next]
h_new = min(h_list.values())
if h_new > h[s]:
h[s] = h_new
s_child = min(h_list, key=h_list.get)
parent[s_child] = s
s = s_child
# path_get = self.extract_path(parent)
return [], visited
def extract_path(self, parent):
path = [self.xG]
s = self.xG
while True:
s = parent[s]
path.append(s)
if s == self.xI:
break
return path
def h(self, s):
heuristic_type = self.heuristic_type
goal = self.xG
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
elif heuristic_type == "euclidean":
return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
else:
print("Please choose right heuristic type!")
@staticmethod
def get_cost(x, u):
"""
Calculate cost for this motion
:param x: current node
:param u: input
:return: cost for this motion
:note: cost function could be more complicate!
"""
return 1
def main():
x_start = (10, 5) # Starting node
x_goal = (45, 25) # Goal node
lrtastar = LrtAstar(x_start, x_goal, "manhattan")
plot = plotting.Plotting(x_start, x_goal) # class Plotting
path, visited = lrtastar.searching()
pathx = [x[0] for x in path]
pathy = [x[1] for x in path]
vx = [x[0] for x in visited]
vy = [x[1] for x in visited]
plot.plot_grid("test")
plt.plot(pathx, pathy, 'r')
plt.plot(vx, vy, 'gray')
plt.show()
if __name__ == '__main__':
main()