-
Notifications
You must be signed in to change notification settings - Fork 1
/
solver_model.py
153 lines (118 loc) · 4.5 KB
/
solver_model.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
import numpy as np
import gworld as world
import cost_heur_astar as ch_astar
from visualize import *
def get_boundwalls(world):
h, w = world.get_size()
bwalls = set()
for x in range(w):
bwalls.add( (0,x) )
bwalls.add( (h-1,x) )
for y in range(h):
bwalls.add( (y,0) )
bwalls.add( (y,w-1) )
return tuple(bwalls)
class SolverModel:
def __init__(self, world, visualize = None):
self.world = world
self.goal_pos = world.goal_pos
self.cost_heur = dict()
self.update_cost_heur()
self.needs_update = False
self.vis = visualize
def update_cost_heur(self):
agents, goals = self.update_goal_pos()
start = []
for agent in agents:
start.append(self.world.aindx_cpos[agent])
self.cost_heur = ch_astar.get_costmat(self.world.get_nbor_cells,
goals,
start,
lambda cell: 1,
lambda cell: cell not in self.world.goal_blocked and not self.world.is_blocked(cell[0], cell[1]),
self.cost_heur )
return self.cost_heur
def update_goal_pos(self):
agents = []
goal_pos = self.goal_pos
for agent in self.world.get_agents():
if(self.world.aindx_goalreached[agent]):
if( self.world.aindx_cpos[agent] in goal_pos ):
goal_pos.remove( self.world.aindx_cpos[agent] )
else:
agents.append( agent )
self.goal_pos = goal_pos
return agents, goal_pos
def agent_greedy_step(self, agent):
cpos = self.world.aindx_cpos[agent]
nbors = self.world.get_nbor_cells(cpos)
nbor0 = nbors[-1]
best_nbor = nbor0
# print self.cost_heur
min_cost = self.cost_heur[nbor0][0]
for nbor in nbors:
if(self.world.passable(nbor) and nbor in self.cost_heur):
tcost = self.cost_heur[nbor][0]
if(tcost < min_cost):
min_cost = tcost
best_nbor = nbor
nxt_action = self.world.pos_to_action(cpos, best_nbor)
return best_nbor, nxt_action
def solve_step(self):
act_agents, goal_pos = self.update_goal_pos()
random.shuffle(act_agents)
for agent in act_agents:
self.update_goal_pos()
if (self.needs_update):
self.update_cost_heur()
self.needs_update = False
best_nbor, nxt_action = self.agent_greedy_step(agent)
self.vis.canvas.update()
self.vis.canvas.after(100)
self.world.agent_action(agent, nxt_action)
if(best_nbor in self.goal_pos):
self.world.aindx_goalreached[agent] = True
self.world.goal_blocked.append( best_nbor )
self.needs_update = True
if __name__ == "__main__":
a = world.GridWorld(15, 15)
bwalls = get_boundwalls(a)
a.add_rocks( bwalls )
# a.add_rocks( ( (1,1),(3,3),(1,3),(3,1),(2,4) ) )
# # a.add_agents( ((3,2),(1,6),(7,8),(2,6),(2,8),(5,6),(4,7)) )
# a.add_agents_rand(7)
# a.add_goal_pos( ( (2,3),(6,1),(8,7),(6,2),(8,2),(6,5),(7,4) ) )
a.add_agents_rand(24)
a.add_goal_pos( ( (3,2),(4,2),(5,2),(2,3),(6,3),(2,4),(6,4), \
(3,6),(4,6),(5,6),(6,6),(2,7),(5,7),(3,8),(4,8),(5,8),(6,8), \
(3,10),(6,10),(2,11),(4,11),(6,11),(2,12),(5,12) \
) )
# a.add_agents_rand(27)
# a.add_goal_pos( ( (2,2),(3,2),(4,2),(3,3),(2,4),(3,4),(4,4), \
# (3,6),(2,7),(3,7),(4,7),(3,8), \
# (2,10),(3,10),(4,10),(3,11),(2,12),(3,12),(4,12),
# (6,2),(6,3),(6,4), (6,6),(6,7),(6,8), (6,10),(6,11),(6,12) \
# ) )
vis = Visualize(a)
vis.draw_world()
vis.draw_agents()
vis.canvas.pack()
vis.canvas.update()
vis.canvas.after(200)
solver = SolverModel(a, vis)
break_loop = False
iter_val = 0
while (True):
print 'iter: ', iter_val
solver.solve_step()
break_loop = True
for agent in a.get_agents():
if (not a.aindx_goalreached[agent]):
break_loop = False
vis.canvas.update()
vis.canvas.after(250)
if(break_loop):
break
iter_val += 1
vis.canvas.update()
vis.canvas.after(2500)