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

nirnayroy · web-flow · commit 2cf4f6f001ec · 2021-05-15T15:01:29.000+09:00
* 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
diff --git a/PathPlanning/DStar/dstar.py b/PathPlanning/DStar/dstar.py
@@ -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()
diff --git a/README.md b/README.md
@@ -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.
diff --git a/docs/modules/path_planning.rst b/docs/modules/path_planning.rst
@@ -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
diff --git a/tests/test_dstar.py b/tests/test_dstar.py
@@ -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__)
