-
Notifications
You must be signed in to change notification settings - Fork 1
/
criticalJSG.py
181 lines (158 loc) · 7.16 KB
/
criticalJSG.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
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
from collections import defaultdict
import itertools
from vanillaDijkstra import Dijkstra
class CJSGraph:
def __init__(self, V):
'''
Instantiate CJSG Graph with the number of vertices of the graph.
'''
# EG variables
self.V = V # nodes/vertices
self.S11 = (None,None) # start position
self.Sgg = (None,None) # goal position
self.adj = [[0 for column in range(self.V)] for row in
range(self.V)] # adj with edge cost and support nodes
self.adj_ns = [[0 for column in range(self.V)] for row in
range(self.V)] # adj with edge cost only
# CJSG variables
self.M = set() # CJSG nodesets
self.H = defaultdict(list) # dict of CJSG nodesets and edgesets
self.graph = [[[] for column in range(V)] for row in
range(V)] # used adjMatrix for Crtical Joint State Graph (CJSG)
# Temoorary variables
self.ESS = set() # add riky edges along with support nodes
# return adjMatrix of CJSG
def get_adjMatrix_2D_CJSG(self):
return self.graph
# return node sets of CJSG
def get_nodeSets_CJSG(self):
return self.M
# add critical nodesets for CJSG
def add_critical_nodesets(self):
for i in range(self.V):
for j in range(self.V):
if self.adj[i][j]!=float('inf') and self.adj[i][j]!=0 and self.adj[i][j][1]!=():
#print((i,j),adj[i][j])
# calculate node sets for CJSG
ES = (i,j) # risky edge with support nodes
Sij=self.adj[i][j][1] # support nodes
self.ESS.add(ES)
print(ES, Sij)
for u in ES:
for v in Sij:
self.M.add((u,v))
self.M.add((v,u))
self.add_start_and_goal()
# add start and goal agents position if not present in the node sets
def add_start_and_goal(self):
if self.S11 not in self.M:
self.M.add(self.S11)
if self.Sgg not in self.M:
self.M.add(self.Sgg)
print("ESS set {}",format(self.ESS))
# calculate edge sets for CJSG
# add adj list of CJSG for node sets with edge cost
# (u,v) one node, (x,y) another node
def addEdge_H(self,u,v,x,y,w):
'''
The Graph will be bidirectional and assumed to have positive weights.
'''
# this works for adjList
if [w,(x,y)] not in self.H[(u,v)] and w!= float('inf'):
self.H[(u,v)].append([w,(x,y)])
if [w,(u,v)] not in self.H[(x,y)] and w!= float('inf'):
self.H[(x,y)].append([w,(u,v)])
# this works for adjMatrix
if [w,(x,y)] not in self.graph[u][v]:
self.graph[u][v].append([w,(x,y)])
if [w,(u,v)] not in self.graph[x][y]:
self.graph[x][y].append([w,(u,v)])
# agents movement without support p->r, q->l
def edge_cost(self,p,r,q,l,support=False):
Rpr = 0
Rql = 0
if self.adj[p][r]!=float('inf') and self.adj[p][r]!=0:
Rpr = self.adj[p][r][0]
else:
Rpr = self.adj[p][r]
if self.adj[q][l]!=float('inf') and self.adj[q][l]!=0:
Rql = self.adj[q][l][0]
else:
Rql = self.adj[q][l]
R_result = Rpr+Rql
if (support):
return R_result/2
return R_result
# convert environt Graph to Critical Joint State Graph
def construct_CJSG(self):
count = 1
for x in self.M:
print("Iteration: {}".format(count))
for y in self.M:
if x!=y: # assumption no self loop
# x: pq and y = rl so x[0]y[0]: pr and x[1]y[1]: ql
p,q = x[0],x[1]
r,l = y[0],y[1]
# edge cost without support
Rpq_rl = self.edge_cost(p,r,q,l)
# edge ql in ES and p==r and belongs to Sql (support nodes for the risky edge ql)
if (q,l) in self.ESS and p==r and p in self.adj[q][l][1]: # Sij
print((p,q),(r,l))
print("QL in ES !!!!!!!!!")
# edge cost with support
Cql_s = self.edge_cost(p,r,q,l, support=True)
w = min(Cql_s,Rpq_rl)
self.addEdge_H(p,q,r,l, w)
# edge pr in ES and q==l and belongs to Spr (support nodes for the risky edge pr)
elif (p,r) in self.ESS and q==l and q in self.adj[p][r][1]:# Sij
print((p,q),(r,l))
Cpr_s = self.edge_cost(p,r,q,l, support=True)
w = min(Cpr_s,Rpq_rl)
print(w)
print("PR in ES !!!!!!!!!")
self.addEdge_H(p,q,r,l, w)
else:
print((p,q),(r,l))
w = Rpq_rl
if w!=float('inf'):
print("Yay!!!!!",w)
self.addEdge_H(p,q,r,l, w)
count=count+1
# shortest path between nodes in EG
def vanilla_dijkstra(self,p,r,q,l):
g = Dijkstra(self.V)
g.graph = self.adj_ns
Rpr = g.dijkstra(src=p,dest=r)
Rql = g.dijkstra(src=q,dest=l)
return Rpr+Rql
# add cjsg edgesets
def add_cjsg_edgesets(self):
for nodex in sorted(self.M):
for nodey in sorted(self.M):
if nodex!= nodey:
present_next_nodes = [next_node for weight, next_node in sorted(self.H[nodex])]
print(nodex, nodey, present_next_nodes)
if nodey not in present_next_nodes:
print(nodey)
p,q = nodex[0],nodex[1]
r,l = nodey[0],nodey[1]
# Rpr+Rql
print(p,r,q,l)
Rpq_rl = self.vanilla_dijkstra(p,r,q,l)
#Rpq_rl = self.edge_cost(p,r,q,l)
self.addEdge_H(p,q,r,l, w=Rpq_rl)
# def add_remaning_cjsg_nodesets(self):
# for nodex, nodey in itertools.product(sorted(self.M), repeat=2):
# if nodex != nodey:
# u, v = nodex
# present_next_nodes = [next_node for weight, next_node in self.graph[u][v]]
# if nodey not in present_next_nodes:
# p, q = nodex[0], nodex[1]
# r, l = nodey[0], nodey[1]
# Rpq_rl = self.vanilla_dijkstra(p, r, q, l)
# self.addEdge_H(p, q, r, l, w=Rpq_rl)
# transform Environment Grapoh to Critical Joint State Graph
def transform_EG_to_CJSG(self):
self.add_critical_nodesets()
self.construct_CJSG()
self.add_cjsg_edgesets()