AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added D* Search to path planning folder (#490)

Browse Source 
* changes

* updated docs and readme

* Update a_star.py

* Update a_star.py

* Create test_dstar.py

* trailing loc

* Update dstar.py

* Update dstar.py

* Update dstar.py

* Update dstar.py

* Update dstar.py

* newline

* corrected changes requested

* 13, five, 21

* corrected changes

* latest

* linted

* lint

* removed diff
This commit is contained in:
nirnayroy
2021-05-15 11:31:29 +05:30
committed by GitHub
parent 0e232037c7
commit 2cf4f6f001
4 changed files with 282 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

241
PathPlanning/DStar/dstar.py Normal file
View File

@@ -0,0 +1,241 @@
"""
D* grid planning
author: Nirnay Roy
See Wikipedia article (https://en.wikipedia.org/wiki/D*)
"""
import math
from sys import maxsize
import matplotlib.pyplot as plt
show_animation = True
class State:
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
self.state = "."
self.t = "new" # tag for state
self.h = 0
self.k = 0
def cost(self, state):
if self.state == "#" or state.state == "#":
return maxsize
return math.sqrt(math.pow((self.x - state.x), 2) +
math.pow((self.y - state.y), 2))
def set_state(self, state):
"""
.: new
#: obstacle
e: oparent of current state
*: closed state
s: current state
"""
if state not in ["s", ".", "#", "e", "*"]:
return
self.state = state
class Map:
def __init__(self, row, col):
self.row = row
self.col = col
self.map = self.init_map()
def init_map(self):
map_list = []
for i in range(self.row):
tmp = []
for j in range(self.col):
tmp.append(State(i, j))
map_list.append(tmp)
return map_list
def get_neighbers(self, state):
state_list = []
for i in [-1, 0, 1]:
for j in [-1, 0, 1]:
if i == 0 and j == 0:
continue
if state.x + i < 0 or state.x + i >= self.row:
continue
if state.y + j < 0 or state.y + j >= self.col:
continue
state_list.append(self.map[state.x + i][state.y + j])
return state_list
def set_obstacle(self, point_list):
for x, y in point_list:
if x < 0 or x >= self.row or y < 0 or y >= self.col:
continue
self.map[x][y].set_state("#")
class Dstar:
def __init__(self, maps):
self.map = maps
self.open_list = set()
def process_state(self):
x = self.min_state()
if x is None:
return -1
k_old = self.get_kmin()
self.remove(x)
if k_old < x.h:
for y in self.map.get_neighbers(x):
if y.h <= k_old and x.h > y.h + x.cost(y):
x.parent = y
x.h = y.h + x.cost(y)
elif k_old == x.h:
for y in self.map.get_neighbers(x):
if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y) \
or y.parent != x and y.h > x.h + x.cost(y):
y.parent = x
self.insert(y, x.h + x.cost(y))
else:
for y in self.map.get_neighbers(x):
if y.t == "new" or y.parent == x and y.h != x.h + x.cost(y):
y.parent = x
self.insert(y, x.h + x.cost(y))
else:
if y.parent != x and y.h > x.h + x.cost(y):
self.insert(y, x.h)
else:
if y.parent != x and x.h > y.h + x.cost(y) \
and y.t == "close" and y.h > k_old:
self.insert(y, y.h)
return self.get_kmin()
def min_state(self):
if not self.open_list:
return None
min_state = min(self.open_list, key=lambda x: x.k)
return min_state
def get_kmin(self):
if not self.open_list:
return -1
k_min = min([x.k for x in self.open_list])
return k_min
def insert(self, state, h_new):
if state.t == "new":
state.k = h_new
elif state.t == "open":
state.k = min(state.k, h_new)
elif state.t == "close":
state.k = min(state.h, h_new)
state.h = h_new
state.t = "open"
self.open_list.add(state)
def remove(self, state):
if state.t == "open":
state.t = "close"
self.open_list.remove(state)
def modify_cost(self, x):
if x.t == "close":
self.insert(x, x.parent.h + x.cost(x.parent))
def run(self, start, end):
rx = []
ry = []
self.open_list.add(end)
while True:
self.process_state()
if start.t == "close":
break
start.set_state("s")
s = start
s = s.parent
s.set_state("e")
tmp = start
while tmp != end:
tmp.set_state("*")
rx.append(tmp.x)
ry.append(tmp.y)
if show_animation:
plt.plot(rx, ry)
plt.pause(0.01)
if tmp.parent.state == "#":
self.modify(tmp)
continue
tmp = tmp.parent
tmp.set_state("e")
return rx, ry
def modify(self, state):
self.modify_cost(state)
while True:
k_min = self.process_state()
if k_min >= state.h:
break
def main():
m = Map(100, 100)
ox, oy = [], []
for i in range(-10, 60):
ox.append(i)
oy.append(-10)
for i in range(-10, 60):
ox.append(60)
oy.append(i)
for i in range(-10, 61):
ox.append(i)
oy.append(60)
for i in range(-10, 61):
ox.append(-10)
oy.append(i)
for i in range(-10, 40):
ox.append(20)
oy.append(i)
for i in range(0, 40):
ox.append(40)
oy.append(60 - i)
print([(i, j) for i, j in zip(ox, oy)])
m.set_obstacle([(i, j) for i, j in zip(ox, oy)])
start = [10, 10]
goal = [50, 50]
if show_animation:
plt.plot(ox, oy, ".k")
plt.plot(start[0], start[1], "og")
plt.plot(goal[0], goal[1], "xb")
start = m.map[start[0]][start[1]]
end = m.map[goal[0]][goal[1]]
dstar = Dstar(m)
rx, ry = dstar.run(start, end)
if show_animation:
plt.plot(rx, ry)
plt.show()
if __name__ == '__main__':
main()

14
README.md
View File

@@ -37,6 +37,7 @@ Python codes for robotics algorithm.
* [Grid based search](#grid-based-search)
* [Dijkstra algorithm](#dijkstra-algorithm)
* [A* algorithm](#a-algorithm)
* [D* algorithm](#d-algorithm)
* [Potential Field algorithm](#potential-field-algorithm)
* [Grid based coverage path planning](#grid-based-coverage-path-planning)
* [State Lattice Planning](#state-lattice-planning)
@@ -301,6 +302,19 @@ In the animation, cyan points are searched nodes.
Its heuristic is 2D Euclid distance.
### D\* algorithm
This is a 2D grid based the shortest path planning with D star algorithm.
![figure at master · nirnayroy/intelligentrobotics](https://github.com/nirnayroy/intelligent-robotics/blob/main/dstar.gif)
The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm.
Ref:
- [D* Algorithm Wikipedia](https://en.wikipedia.org/wiki/D*)
### Potential Field algorithm
This is a 2D grid based path planning with Potential Field algorithm.

16
docs/modules/path_planning.rst
View File

@@ -39,6 +39,21 @@ In the animation, cyan points are searched nodes.
Its heuristic is 2D Euclid distance.
.. _a*-algorithm:
D\* algorithm
~~~~~~~~~~~~~
This is a 2D grid based shortest path planning with D star algorithm.
|dstar|
The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm.
Ref:
- `D* search Wikipedia <https://en.wikipedia.org/wiki/D*>`__
Potential Field algorithm
~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -427,6 +442,7 @@ Ref:
.. |DWA| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif
.. |Dijkstra| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/Dijkstra/animation.gif
.. |astar| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/AStar/animation.gif
.. |dstar| image:: https://github.com/nirnayroy/intelligent-robotics/blob/main/dstar.gif
.. |PotentialField| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/PotentialFieldPlanning/animation.gif
.. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/kn05animation.gif
.. |5| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png?raw=True

11
tests/test_dstar.py Normal file
View File

@@ -0,0 +1,11 @@
import conftest
from PathPlanning.DStar import dstar as m
def test_1():
m.show_animation = False
m.main()
if __name__ == '__main__':
conftest.run_this_test(__file__)