# Ch. 3
###########################################################
# 3.1 Optimal Search
###########################################################
# 状態空間 { ノード: [ (隣のノード, コスト) ... ] , ... }
StateMap = {'s': [('a',2), ('b',6)],
'a':[('s',2),('b',2),('c',1)],
'b': [('s',6),('a',2),('e',5),('f',4)],
'c':[('a',1),('d',5),('e',2)],
'd':[('c',5),('e',2),('g',1)],
'e':[('b',5),('c',2),('d',1),('g',5)],
'f':[('b',4)],
'g':[('d',1),('e',5)]}
def optimal_search(stateMap, start, goal):
closed = {} # sep 1
cost = 0 # ditto
open = [(cost, start)] # ditto
while ( open != [] ): # step 2
print ('open = ', open )
node = open[0]; open = open[1:] # step 3 (pop)
[cost, state] = node # state
closed[state]=cost
#
if (state == goal): # step 4
print('reached the goal')
return closed
open = sorted(append_to_open(open,state,closed,cost, stateMap)) # step 5
###
def append_to_open(open, state, closed, cost, stateMap):
new_open = {}
adjacents = stateMap[state]
for (c,s) in open:
new_open[s]=c
#
while (adjacents != []):
[next_state,added_cost] = adjacents[0]
adjacents = adjacents[1:]
if not (next_state in closed):
new_cost = cost+added_cost
if not (next_state in new_open):
new_open[next_state] = new_cost
elif new_open[next_state] > new_cost:
new_open[next_state] = new_cost
return [(c,s) for (s,c) in new_open.items()]
# 最適探索
print ("optimal search...")
optimal_search(StateMap,'s','g')
# これだと経路が出力されない
def optimal_search_R(map, start, goal):
closed = {}
cost = 0
open = [(cost, start, start)] # 3番目は直前のノード
while ( open != [] ):
print ('open = ', open)
node = open[0]; open = open[1:] # pop
[cost, state, prec] = node # state
closed[state]=(cost,prec) # (cost, 前のノード)
#
if (state == goal):
print('reached the goal')
return closed
open = sorted(append_to_open_R(open, map, state,closed,cost))
def append_to_open_R(open, map, state, closed, cost):
adjacents = map[state]
new_open = {}
for (c,s,pre) in open:
new_open[s]=(c,pre)
#
while (adjacents != []):
[next_state,added_cost] = adjacents[0]
adjacents = adjacents[1:]
if not (next_state in closed):
new_cost = cost+added_cost
if not (next_state in new_open):
new_open[next_state] = (new_cost, state)
elif new_open[next_state][0] > new_cost:
new_open[next_state] = (new_cost, state)
return [(c,s,p) for (s,(c,p)) in new_open.items()]
# optimal_search_R(StateMap,'s','g')
#############################################
# 経路表示
#############################################
def showRoute(result, start, goal):
node=goal
route = []
print ('cost = ',result[goal][0] )
while(node != start):
route = [node]+route
nextNode = result[node]
node = nextNode[-1]
for x in [node]+route[:-1]:
print ( x,'->',end="")
print ( goal )
showRoute(optimal_search_R(StateMap,'s','g'),'s','g')
###########################################################
# 3.2 Best-first Search
###########################################################
# 予測評価値
Predicted_costs = {'a':4, 'b':2, 'c':3, 'd':1, 'e':1,
'f':0, 'g':0, 's':4}
def bf_search(stateMap, predicted_cost_Map, start, goal):
closed = {} # step 1
cost = 0 # ditto
hcost = predicted_cost_Map[start] # ditto
open = [(hcost, start, cost)] # ditto
while ( open != [] ): # step 2
print ('open = ', open )
node = open[0]; open = open[1:] # step 3 (pop)
[hcost, state, cost] = node # state
closed[state]=cost
#
if (state == goal): # step 4
print('reached the goal')
return closed
# step 5
open = sorted(append_to_open_BF(open,
state,closed,cost, stateMap,predicted_cost_Map))
###
def append_to_open_BF(open, state, closed,cost,stateMap, predicted_cost_Map):
adjacents=StateMap[state]
new_open = {}
for (h,s,c) in open:
new_open[s]=(h,c) # (predicted, real_cost)
#
while (adjacents != []):
[next_state,added_cost] = adjacents[0]
adjacents = adjacents[1:]
if not (next_state in closed):
new_cost = cost+added_cost
if not (next_state in new_open):
new_open[next_state] = (predicted_cost_Map[next_state],new_cost)
elif new_open[next_state][1] > new_cost:
new_open[next_state] = (new_open[next_state][0],new_cost)
return [(h,s,c) for (s,(h,c)) in new_open.items()]
# 最良優先探索
print ("\n\nBest First Search...")
bf_search(StateMap,Predicted_costs,'s','g')
####################################################
# 経路表示
def bf_search_R(stateMap, predicted_cost_Map, start, goal):
closed = {}
cost = 0
hcost = predicted_cost_Map[start] #
open = [(hcost, start, cost, start)] #
while ( open != [] ):
print ('open = ', open)
node = open[0]; open = open[1:] # pop
[hcost, state, cost, pre] = node # state
closed[state]=(cost, pre)
#
if (state == goal):
print('reached the goal')
return closed
open = sorted(append_to_open_BF_R(open,
state,closed,cost, stateMap, predicted_cost_Map))
def append_to_open_BF_R(open, state, closed,cost, stateMap, predicted_cost_Map):
adjacents=StateMap[state]
new_open = {}
for (h,s,c,p) in open:
new_open[s]=(h,c,p) # (predicted, real_cost, precedingState)
#
while (adjacents != []):
[next_state,added_cost] = adjacents[0]
adjacents = adjacents[1:]
if not (next_state in closed):
new_cost = cost+added_cost
if not (next_state in new_open):
new_open[next_state] = (predicted_cost_Map[next_state],new_cost,state)
elif new_open[next_state][1] > new_cost:
new_open[next_state] = (new_open[next_state][0],new_cost,state)
return [(h,s,c,p) for (s,(h,c,p)) in new_open.items()]
showRoute(bf_search_R(StateMap,Predicted_costs,'s','g'),'s','g')
###########################################################
# 3.3 A-star
###########################################################
def aStar_search(stateMap, predicted_costMap, start, goal):
closed = {} # step 1
cost = 0 # ditto
hcost = predicted_costMap[start]+cost # ditto
open = [(hcost, start, cost)] # ditto
while ( open != [] ): # step 2
print ('open = ', open)
node = open[0]; open = open[1:] # step 3 (pop)
[hcost, state, cost] = node # state
closed[state]=(hcost,cost)
# step 5,6,7,8
open = sorted(append_to_open_AS(open,
state,closed,cost,stateMap, predicted_costMap))
return closed
def append_to_open_AS(open, state, closed,cost,stateMap, predicted_costMap):
adjacents=stateMap[state]
new_open = {}
for (h,s,c) in open:
new_open[s]=(h,c) # (predicted, real_cost)
#
while (adjacents != []):
[next_state,added_cost] = adjacents[0]
adjacents = adjacents[1:]
new_cost = cost+added_cost # step 5
hcost = new_cost+predicted_costMap[next_state]
if (next_state in closed): # step 7
if closed[next_state][0] > hcost:
closed[next_state] = (hcost,new_cost)
else:
if not (next_state in new_open): # step 6
new_open[next_state] = (hcost, new_cost)
elif new_open[next_state][0] > hcost: # step 7
new_open[next_state] = (hcost,new_cost)
return [(h,s,c) for (s,(h,c)) in new_open.items()]
# A* 探索
print ("\n\nA* Search")
aStar_search(StateMap,Predicted_costs,'s','g')
# 問題点: Gに達するパスが表示されない
# 解決策: どこから来たかというノードを記憶する
def aStar_search_R(stateMap,predicted_costMap, start, goal):
closed = {}
cost = 0
hcost = predicted_costMap[start]+cost #
open = [(hcost, start, cost, 0)] # 0 means start
while ( open != [] ):
print ('open = ', open)
node = open[0]; open = open[1:] # pop
[hcost, state, cost, prec] = node # state
closed[state]=(hcost,cost,prec)
#
# if (state == goal):
# print('reached the goal')
# return closed
open = sorted(append_to_open_AS_R(open,state,
closed,cost,stateMap, predicted_costMap))
return closed
def append_to_open_AS_R(open,state,closed,cost,stateMap, predicted_costMap):
new_open = {}
for (h,s,c,p) in open:
new_open[s]=(h,c,p) # (predicted, real_cost, last_state)
#
adjacents = stateMap[state]
while (adjacents != []):
[next_state,added_cost] = adjacents[0]
adjacents = adjacents[1:]
new_cost = cost+added_cost
hcost = new_cost+predicted_costMap[next_state]
if (next_state in closed):
if closed[next_state][0] > hcost:
closed[next_state] = (hcost,new_cost,state)
else:
if not (next_state in new_open):
new_open[next_state] = (hcost, new_cost,state)
elif new_open[next_state][0] > hcost:
new_open[next_state] = (hcost,new_cost,state)
return [(h,s,c,p) for (s,(h,c,p)) in new_open.items()]
# aStar_search_Mod(StateMap,Predicted_costs,'s','g')
showRoute(aStar_search_R(StateMap,Predicted_costs,'s','g'),'s','g')
##################################
Fig3_3 = {'s':(0,0), 's1':(2,0), 's2':(5,0),
's3':(1,1), 's4':(4,1),
's5':(2,2), 's6':(5,2),
's7':(2,3), 's8': (3,3),
's9':(1,4), 's10':(3,4), 'g':(6,4)}
Fig3_3States = {'s':['s3'], 's1':['s4'], 's2':['s6'],
's3':['s','s4','s7'], 's4':['s1','s3','s6'],
's5':['s8'], 's6':['s4','s2','g'],
's7':['s3','s8','s9'], 's8':['s7','s5','s10'],
's9':['s7'], 's10':['s8'], 'g':['s6']}
def predicted_cost(node, goal='g'):
(gx, gy)=Fig3_3[goal]
(x,y)=Fig3_3[node]
return abs(x-gx)+abs(y-gy)
def cost2nextNode(n, m):
if (n,m) == ('s6','g') or (m,n)==('s6','g'):
return 5 # special case
return predicted_cost(n,m)
def testCost(nodes):
for n in nodes:
adjacents = Fig3_3States[n]
for m in adjacents:
print ( n, 'to', m, '=', cost2nextNode(n,m) )
def aStar_search_Fig3_3(stateMap, start, goal):
closed = {}
cost = 0
hcost = predicted_cost(start)+cost #
open = [(hcost, start, cost, 0)] # 0 means start
while ( open != [] ):
print ('open = ', open )
node = open[0]; open = open[1:] # pop
[hcost, state, cost, prec] = node # state
closed[state]=(hcost,cost,prec)
open = sorted(append_to_open_AS_Fig3_3(open,closed,cost,state,stateMap))
return closed
def append_to_open_AS_Fig3_3(open,closed,cost,state,stateMap):
new_open = {}
for (h,s,c,p) in open:
new_open[s]=(h,c,p) # (predicted, real_cost, last_state)
#
adjacents = stateMap[state]
while (adjacents != []):
next_state = adjacents[0]
added_cost=cost2nextNode(state,next_state)
adjacents = adjacents[1:]
new_cost = cost+added_cost
hcost = new_cost+predicted_cost(next_state)
if (next_state in closed):
if closed[next_state][0] > hcost:
closed[next_state] = (hcost,new_cost,state)
else:
if not (next_state in new_open):
new_open[next_state] = (hcost, new_cost,state)
elif new_open[next_state][0] > hcost:
new_open[next_state] = (hcost,new_cost,state)
return [(h,s,c,p) for (s,(h,c,p)) in new_open.items()]
# Fig3.3
print ("\n\nFigure 3.3 Search" )
showRoute(aStar_search_Fig3_3(Fig3_3States,'s','g'),'s','g')