Motion Planning
Given: Map, starting location, goal location, cost
Goal: Find minimum cost
Each Actions: move, turn
grid = [[0, 0, 1, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 1, 1, 0],
[0, 0, 0, 0, 1, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1
delta = [[-1, 0],
[0, -1],
[1, 0],
[0, 1]]
delta_name = ['^','<','v','>']
def search(grid, init, goal,cost):
closed = [[0 for row in range(len(grid[0]))] fro col in range(len(grid))]
x = init[0]
y = init[1]
g = 0
open = [[g, x, y]]
return path
