Breadth-First Search

LICENSE

@@ -1,5 +1,5 @@
 MIT License
-Copyright (c) <year> <copyright holders>
+Copyright (c) 2021 Zhilong Li
 
 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
 

Motion Planning/first_search.py

@@ -0,0 +1,77 @@
+# ----------
+# User Instructions:
+#
+# Define a function, search() that returns a list
+# in the form of [optimal path length, row, col]. For
+# the grid shown below, your function should output
+# [11, 4, 5].
+#
+# If there is no valid path from the start point
+# to the goal, your function should return the string
+# 'fail'
+# ----------
+
+# Grid format:
+#   0 = Navigable space
+#   1 = Occupied space
+
+from collections import deque
+import numpy as np
+
+grid = np.array([[0, 0, 1, 0, 0, 0, 0],
+                 [0, 0, 1, 0, 1, 0, 1],
+                 [1, 0, 0, 0, 0, 0, 0],
+                 [0, 0, 1, 1, 1, 0, 0],
+                 [0, 0, 0, 0, 1, 0, 1]])
+init = np.array([0, 0])
+# goal = np.array([len(grid)-1, len(grid[0])-1])
+goal = np.array([3, 6])
+cost = np.array([1.0, 1.0, 1.0, 1.0])
+
+delta = np.array([[-1, 0],  # go up
+                  [0, -1],  # go left
+                  [1, 0],  # go down
+                  [0, 1]])  # go right
+
+delta_name = ['^', '<', 'v', '>']
+
+
+def check_navigable(goal) -> bool:
+    if all(goal >= np.array([0, 0])) and all(goal <= np.array([len(grid)-1, len(grid[0])-1])):
+        # print("G: ",goal)
+        if not grid[goal[0], goal[1]]:
+            return True
+    return False
+
+
+def search(grid: list, init: list, goal: list, cost: list):
+    # ----------------------------------------
+    # insert code here
+    # ----------------------------------------
+    path = list()
+    next_check = deque()
+    next_check.append(init)
+    already_checked = dict()
+    already_checked[tuple(init)] = 0
+    cost_map = np.zeros([len(grid), len(grid[0])])
+
+    while next_check:
+        checking = next_check.popleft()
+        if check_navigable(checking):
+            # already_checked[tuple(checking)] =
+            for move in delta:
+                next = checking+move  # type: np.ndarray
+                if check_navigable(next) and tuple(next) not in already_checked:
+                    next_check.append(next)
+                    cost_now = already_checked[checking[0], checking[1]]+1
+                    already_checked[tuple(next)] = cost_now
+                    cost_map[next[0], next[1]] = cost_now
+                    if all(next == goal):
+                        print(cost_map)
+                        return [cost_now, next[0], next[1]]
+
+    print(cost_map)
+    return "fail"
+
+
+print(search(grid, init, goal, cost))