-
Notifications
You must be signed in to change notification settings - Fork 2
/
smart_selectors.py
308 lines (276 loc) · 10.9 KB
/
smart_selectors.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
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
#!/usr/bin/python2
#
# Author: Blake Hannaford
#
# A set of selector nodes which use different methods to choose which child to tick first.
#
import b3
import random
#########################################################################
#
# Selector 0: Choose children in order they are listed (traditional selector)
#
#########################################################################
class SmrtSel00(b3.Composite):
def __init__(self, children=None):
super(SmrtSel00, self).__init__(children)
def tick(self, tick):
if(self.BHdebug == 1):
print "Sel 0: Node ticked: ", self.Name
children = []
# go through the children and get estimated P(s) for each
for node in self.children:
#print "Selecting from "+node.Name
# prob independent of state / experience
#p = tick.blackboard.get('thresh', tick.tree.id, node.id)
# prob depends on experience only
node.P_selector = node.prob()
#print "Returned probability: ",p
children.append(node)
# for 00 DO NOT sort the probabilities (lowest first)
#rank = sorted(children, key=lambda node: node.P_selector) # sort by P(s|F)
rank = children
if(self.BHdebug == 1):
print "dumb selector (sel00): fixed order"
N = len(rank)
tmpCost = 0
for j in range(0,N):
# Try them in fixed order
if(self.BHdebug == 1):
#if(True):
print "Sel00:",self.Name," ticking "+rank[j].Name
#print 'Sel00: cost: ',self.Cost
status = rank[j]._execute(tick)
tmpCost += rank[j].Cost
self.Cost = tmpCost # my cost equals total leaf costs
if status != b3.FAILURE:
if(self.BHdebug == 1):
print "Sel00: ",self.Name, "->", rank[j].Name, " Succeeded"
return status
if(self.BHdebug == 1):
print "Sel00: ",self.Name, "->", rank[j].Name, " Failed"
return b3.FAILURE
#########################################################################
#
# Selector 1: Choose children in order of estimated probability(success)
#
#########################################################################
class SmrtSel01(b3.Composite):
def __init__(self, children=None):
super(SmrtSel01, self).__init__(children)
def tick(self, tick):
if(self.BHdebug == 1):
print "Sel01: Node ticked: ", self.Name
children = []
# go through the children and get estimated P(s) for each
for node in self.children:
#print "Selecting from "+node.Name
# prob independent of state / experience
#p = tick.blackboard.get('thresh', tick.tree.id, node.id)
# prob depends on experience only
node.P_selector = node.prob()
#print "Returned probability: ",p
children.append(node)
# sort the probabilities (lowest first)
rank = sorted(children, key=lambda node: node.P_selector) # sort by P(s|F)
if(self.BHdebug == 1):
tmpstr = "smart selector 01: P(s) Rank: | "
for n in rank:
tmpstr += n.Name + " " +str(n.P_selector) + " |"
print tmpstr
N = len(rank)
tmpCost = 0
tmpUtil = 0.0
for j in range(0,N):
# Try them in descending order of probability
if(self.BHdebug == 1):
print "Sel01:",self.Name," ticking "+rank[N-1-j].Name
status = rank[N-1-j]._execute(tick)
tmpCost += rank[N-1-j].Cost
if status != b3.FAILURE:
if(self.BHdebug == 1):
print "Sel01: ",self.Name, "->", rank[N-1-j].name, " Succeeded"
self.Cost = tmpCost # my cost equals total leaf costs
return status
if(self.BHdebug == 1):
print "Sel01: ",self.Name, "->", rank[N-1-j].name, " Failed"
self.Cost = tmpCost # my cost equals total leaf costs
return b3.FAILURE
#########################################################################
#
# Selector 2: Choose children in order of Ps(Node,state) = P_i(S|F)
#
#########################################################################
class SmrtSel02(b3.Composite):
def __init__(self, children=None):
super(SmrtSel02, self).__init__(children)
self.Greedy=False
def tick(self, tick):
if(self.BHdebug == 1):
print "Sel 02: Node ticked: ", self.Name, " Greedy: ", self.Greedy
children = []
j = 0
for node in self.children:
plist = node.prob_state() # get P(s) for this node and all states
node.get_state(tick.blackboard) # update sensing state (node.state) for this node
node.P_selector = plist[node.state] # this specific state
children.append(node)
rank = sorted(children, key=lambda node: node.P_selector) # sort by P(s|F)
if(self.BHdebug == 1):
tmpstr = "smart selector 02: State = " + str(rank[0].state) +" P(s|F) Rank: | "
for n in rank:
tmpstr += n.Name +": "+ str(n.P_selector)+ " |"
print tmpstr
N = len(rank)
if(self.BHdebug==1):
print "smart sel 02: ticking leaf: ", rank[N-1].Name
tmpCost = 0
if(self.Greedy): # only execute the hightest ranked leaf
tmpCost = rank[N-1].Cost
status = rank[N-1]._execute(tick)
if(self.BHdebug==1):
print "Greedy 02: state: ",node.state, " status: ", status, " cost: ", tmpCost
self.Cost = tmpCost
return status
else:
for j in range(0,N): # execute the children in order of probability
if(self.BHdebug == 1):
print "Sel02: I'm ticking ", rank[N-1-j].Name
tmpCost += rank[N-1-j].Cost
status = rank[N-1-j]._execute(tick)
if status != b3.FAILURE:
if(self.BHdebug == 1):
print self.Name,": State: ", rank[N-1-j].state , " SUCCESS "
self.Cost = tmpCost # my cost equals total leaf costs
return status
self.Cost = tmpCost # my cost equals total leaf costs
return b3.FAILURE
#########################################################################
#
# Selector 3: Choose children in order of Cost(Node)
#
#########################################################################
class SmrtSel03(b3.Composite):
def __init__(self, children=None):
super(SmrtSel03, self).__init__(children)
def tick(self, tick):
if(self.BHdebug == 1):
print "Sel 03: Node ticked: ", self.Name
children = []
#if (self.Name == "Extinguish"):
#print "Extinguish: "
j = 0
for node in self.children:
#print "I'm looking at leaf: "+node.Name
u = node.Cost # get cost for this Leaf
if(self.BHdebug == 1):
print "Sel 03: Leaf Cost: ", u
children.append(node)
rank = sorted(children, key=lambda node: node.Cost) # sort by Cost
if(self.BHdebug == 1):
tmpstr = "smart selector 03: Cost Rank: | "
for n in rank:
tmpstr += n.Name + " " + str(n.Cost) + " |"
print tmpstr
N = len(rank)
tmpCost = 0
for j in range(0,N): # execute the children in ASCENDING order of Cost
if(self.BHdebug == 1):
print "Sel03: I'm ticking ", rank[j].Name
tmpCost += rank[j].Cost
status = rank[j]._execute(tick)
if status != b3.FAILURE:
if(self.BHdebug == 1):
print self.Name, " SUCCESS "
self.Cost = tmpCost # my cost equals total leaf costs
return status
self.Cost = tmpCost # my cost equals total leaf costs
return b3.FAILURE
#########################################################################
#
# Selector 4: Choose children in order of Utility(Node)
#
#########################################################################
class SmrtSel04(b3.Composite):
def __init__(self, children=None):
super(SmrtSel04, self).__init__(children)
def tick(self, tick):
if(self.BHdebug == 1):
print "Sel 04: Node ticked: ", self.Name
state = tick.blackboard.get('State')
children = []
#if (self.Name == "Extinguish"):
#print "Extinguish: "
j = 0
for node in self.children:
#print "I'm looking at leaf: "+node.Name
u = node.get_Utility() # get Utility for this Leaf
if(self.BHdebug == 1):
print "Sel 04: ", node.Name, " Leaf Util: ", u
children.append(node)
rank = sorted(children, key=lambda node: node.Utility) # sort by Utility
if(self.BHdebug == 1):
tmpstr = "smart selector 04: Utility Rank: | "
for n in rank:
tmpstr += n.Name + " " + str(n.Utility) + " |"
print tmpstr
N = len(rank)
tmpCost = 0
for j in range(0,N): # execute the children in descending order of Utility
if(self.BHdebug == 1):
print "Sel04: I'm ticking ", rank[N-1-j].Name
tmpCost += rank[N-1-j].Cost
status = rank[N-1-j]._execute(tick)
if status != b3.FAILURE:
if(self.BHdebug == 1):
print self.Name,": State: ", state , " SUCCESS "
self.Cost = tmpCost # my cost equals total leaf costs
return status
self.Cost = tmpCost # my cost equals total leaf costs
return b3.FAILURE
#########################################################################
#
# Selector 5: Choose children in order of Utility(Node, State)
#
# USE THIS ONLY ON LEAVES (because only they have states)
#
#########################################################################
class SmrtSel05(b3.Composite):
def __init__(self, children=None):
super(SmrtSel05, self).__init__(children)
def tick(self, tick):
if(self.BHdebug == 1):
print "Sel 05: Node ticked: ", self.Name
state = tick.blackboard.get('State')
children = []
#if (self.Name == "Extinguish"):
#print "Extinguish: "
j = 0
for node in self.children:
#print "I'm looking at leaf: "+node.Name
node.get_state(tick.blackboard) # update node's sensing
u = node.get_Utility2() # get Utility for this Leaf conditioned on state
p = node.prob_state()[node.state]
if(self.BHdebug == 1):
print "Sel 05: ", node.Name, "State: ", node.state, "Leaf Prob:", p ," Leaf Util: ", u
children.append(node)
rank = sorted(children, key=lambda node: node.Utility) # sort by Utility
if(self.BHdebug == 1):
tmpstr = "smart selector 05: Utility Rank: | "
for n in rank:
tmpstr += n.Name + " " + str(n.Utility) + " |"
print tmpstr
N = len(rank)
tmpCost = 0
for j in range(0,N): # execute the children in descending order of Utility
if(self.BHdebug == 1):
print "Sel05: I'm ticking ", rank[N-1-j].Name
tmpCost += rank[N-1-j].Cost
status = rank[N-1-j]._execute(tick)
if status != b3.FAILURE:
if(self.BHdebug == 1):
print self.Name,": State: ", state , " SUCCESS "
self.Cost = tmpCost # my cost equals total leaf costs
return status
self.Cost = tmpCost # my cost equals total leaf costs
return b3.FAILURE