python动态规划和A*算法

#动态规划
grid=[[0,1,0,0,0,0],
      [0,0,1,0,0,0],
      [0,0,1,0,0,0],
      [0,0,0,1,1,0],
      [0,0,1,1,1,0],
      [0,0,0,0,1,0]]

init = [len(grid)-1, len(grid[0])-1]

#up left down right
dir = [[0, -1],
      [-1, 0],
      [0, 1],
      [1, 0]]
dir_pic=['<','^','>','v']
cost=1

def search_policy(grid,init,cost):
    gird_row = len(grid)
    gird_col = len(grid[0])
    grid_value = [[99for col in range(gird_col)]for row in range(gird_row)]
    pic=[[' 'for col in range(gird_col)]for row in range(gird_row)]
    change=True
    while change:

        change=False

        for row in range(gird_row):
            for col in range(gird_col):
                if row==init[0] and col==init[1]:
                    if grid_value[row][col]>0:
                        grid_value[row][col]=0
                        pic[row][col]='*'
                        change=True
                elif grid[row][col]==0:
                    for i in range(len(dir)):
                        next_row = row + dir[i][0]
                        next_col = col + dir[i][1]
                        if next_row >=0 and next_row <gird_row and next_col>=0 and next_col<gird_col and grid[next_row][next_col]==0:
                            next_value=grid_value[next_row][next_col]+cost
                            if next_value<grid_value[row][col]:
                                grid_value[row][col]=next_value
                                pic[row][col]=dir_pic[i]
                                change = True
    # for i in grid_value:
    #     print(i)
    for i in range(len(pic)):
        print(pic[i])
    return grid_value
search_policy(grid,init,cost)

 

#A*
#create grid
grid=[[0,0,0,0,0,0],
      [0,1,1,1,1,0],
      [0,1,0,0,0,0],
      [0,1,0,0,0,0],
      [0,1,0,0,1,0]]
heuristic=[[9,8,7,6,5,4],
           [8,7,6,5,4,3],
           [7,6,5,4,3,2],
           [6,5,4,3,2,1],
           [5,4,3,2,1,0]]
init = [0, 0]
goal = [len(grid)-1,len(grid[0])-1]
#up left down right
dir = [[0, -1],
      [-1, 0],
      [0, 1],
      [1, 0]]
dir_pic=['<','^','>','v']
gird_row=len(grid)
gird_col=len(grid[0])


def search(gird, init, goal):
    step = 1
    if init == goal:
        return 0
    #未扩展的标记0，已经扩展的标记1
    expanded_grid=[[0, 0, 0, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0],
                   [0, 0, 0, 0, 0, 0]]
    for row in range(len(grid)):
        for col in range(len(grid[0])):
            if grid[row][col]==1:
                expanded_grid[row][col]='x'
    #初始节点标记
    expanded_grid[init[0]][init[1]] = 1
    # expanded node
    open_list = [[9,0,init[0],init[1]]]
    # cost,list of(x,y)
    path = []
    cost = 1
    #loop
    while open_list != []:
            g = open_list[0][1]
            x = open_list[0][2]
            y = open_list[0][3]
            #弹出最小得f
            path.append(open_list[0])
            del open_list[0]
            if x != goal[0] or y != goal[1]:
                for i in dir:
                    x2 = x+i[0]
                    y2 = y+i[1]
                    # 移动的时候越界判断&障碍物判断&已经扩展过的判断
                    if (x2 <= gird_row-1) and (x2 >= 0) and (y2 <= gird_col-1) and (y2 >= 0) and (gird[x2][y2] == 0) and (expanded_grid[x2][y2] == 0):
                        g2 = g+cost
                        h2 = heuristic[x2][y2]
                        f2 = g2+h2
                        open_list.append([f2, g2, x2, y2])
                        step += 1
                        expanded_grid[x2][y2] = step
                        #将g按从小到大得顺序排列
                        open_list.sort()
            else:
                return expanded_grid
    # print("There is no vaild path!")
    return expanded_grid
expanded=(search(grid,init,goal))
for i in expanded:
    print(i)