def __init__(self, outputFile, discretizer):
self.f = outputFile
self.discretizer = discretizer
self.behaviors = ["explore", "faceObject", "tryGrab", "release"]
self.stateBehaviors = {} # State -> Behavior -> Count
self.algorithmState = "random"
self.graph = MarkovChainGraph()
self.search = DepthFirstSearch()
self.randomMoves = 0
self.randomBehaviors = ["explore"] * 80 + ["faceObject"] * 10 + ["tryGrab"] * 5 + ["release"] * 5
self.sinceLastNewState = 0
self.previous_state = None
self.previous_behavior = None
self.last_search_behavior = None
self.same_search_behavior = 0
self.round = 0
import matplotlib.pyplot as plt
stateTranslate = {
'0.0, 0.0, 0.0': 'nothing to see',
'1.0, 0.0, 0.0': 'object in front',
'1.0, 1.0, 0.0': 'facing object',
'0.0, 0.0, 1.0': 'object grabbed',
'1.0, 0.0, 1.0': 'object grabbed and object in front',
'1.0, 1.0, 1.0': 'object grabbed and facing object',
}
if __name__ == '__main__':
plt.rcParams.update({'figure.autolayout': True})
filename = sys.argv[1]
sensorStates, behaviors = dataloader.load(filename)
discretizer = SimplifyingDiscretizer()
graph = MarkovChainGraph()
result = {}
ss = []
for sensorState in sensorStates:
sensorState = normalize(sensorState)
s = discretizer.discretize(sensorState)
s = graph.state_to_key(s)
ss.append(stateTranslate[s])
if stateTranslate[s] in result:
result[stateTranslate[s]] += 1
else:
result[stateTranslate[s]] = 1
graph.construct(ss, behaviors)
graph.visualize(filename + '.dot', ss[0])
class SmartCollector:
def __init__(self, outputFile, discretizer):
self.f = outputFile
self.discretizer = discretizer
self.behaviors = ["explore", "faceObject", "tryGrab", "release"]
self.stateBehaviors = {} # State -> Behavior -> Count
self.algorithmState = "random"
self.graph = MarkovChainGraph()
self.search = DepthFirstSearch()
self.randomMoves = 0
self.randomBehaviors = ["explore"] * 80 + ["faceObject"] * 10 + ["tryGrab"] * 5 + ["release"] * 5
self.sinceLastNewState = 0
self.previous_state = None
self.previous_behavior = None
self.last_search_behavior = None
self.same_search_behavior = 0
self.round = 0
def getInitialState(self, sensorState):
self.initialState = self.discretizer.discretize(normalize(sensorState))
return self.nextBehavior(sensorState)
def eventHandler(self, state, sensorState, behaviorResult):
return self.nextBehavior(sensorState)
def selectBehavior(self, stateBehaviors):
for b in self.behaviors:
if b not in stateBehaviors:
return b
minpair = (maxint, "")
for behavior, count in stateBehaviors.iteritems():
if count < minpair[0]:
minpair = (count, behavior)
return minpair[1]
def minCountState(self):
mincount = maxint
minstate = ""
for state, behaviors in self.stateBehaviors.iteritems():
if mincount > behaviors["count"]:
mincount = behaviors["count"]
minstate = state
return (minstate, mincount)
def randomBehavior(self, dis, discretized):
if self.stateBehaviors[dis]["count"] == 0 or self.randomMoves > 20:
self.randomMoves = 0
self.algorithmState = "intelligent"
return self.intelligentBehavior(dis, discretized)
else:
self.randomMoves += 1
return random.choice(self.randomBehaviors)
def intelligentBehavior(self, dis, discretized):
if self.sinceLastNewState > 20:
self.sinceLastNewState = 0
self.algorithmState = "random"
return self.randomBehavior(dis, discretized)
if self.stateBehaviors[dis]["count"] > (self.minCountState()[1] + 20):
self.sinceLastNewState = 0
self.algorithmState = "search"
self.goal_state, self.goal_state_count = self.minCountState()
return self.searchBehavior(dis, discretized)
self.sinceLastNewState += 1
return self.selectBehavior(self.stateBehaviors[dis])
def searchBehavior(self, dis, discretized):
if dis == self.goal_state or dis not in self.stateBehaviors:
self.goal_state = None
self.goal_state_count = None
self.algorithmState = "intelligent"
return self.intelligentBehavior(dis, discretized)
problem = FindStateProblem(self.goal_state)
behavior, err = self.search.choose_behavior([discretized], self.graph, problem)
print("Searching for:", self.goal_state)
if err <= 0 or self.same_search_behavior > 5:
print("No path, random")
self.same_search_behavior = 0
return random.choice(self.randomBehaviors)
if self.last_search_behavior == behavior:
self.same_search_behavior += 1
else:
self.last_search_behavior = behavior
self.same_search_behavior = 0
return behavior
def nextBehavior(self, sensorState):
self.round += 1
print("\nRound:", self.round)
print("Algorithm:", self.algorithmState)
print(sensorState, file=self.f)
normSensorState = normalize(sensorState)
discretized = self.discretizer.discretize(normSensorState)
dis = self.graph.state_to_key(discretized)
if self.previous_state is not None and self.previous_behavior is not None:
self.graph.construct([self.previous_state, discretized], [self.previous_behavior])
if dis not in self.stateBehaviors:
self.sinceLastNewState = 0
self.stateBehaviors[dis] = {}
self.stateBehaviors[dis]["count"] = 0
self.stateBehaviors[dis]["origin"] = discretized
behavior = "explore"
if self.algorithmState == "random":
behavior = self.randomBehavior(dis, discretized)
elif self.algorithmState == "intelligent":
behavior = self.intelligentBehavior(dis, discretized)
elif self.algorithmState == "search":
behavior = self.searchBehavior(dis, discretized)
if behavior not in self.stateBehaviors[dis]:
self.stateBehaviors[dis][behavior] = 0
self.stateBehaviors[dis][behavior] += 1
self.stateBehaviors[dis]["count"] += 1
print(self.stateBehaviors)
print("State:", dis)
print("Behavior:", behavior)
print(behavior, file=self.f)
self.previous_state = discretized
self.previous_behavior = behavior
self.graph.visualize("dot/graph.dot", self.initialState)
self.graph.visualize("dot/graph_%d.dot" % self.round, self.initialState)
return behavior
