Пример #1
0
    def addIteratively(self, dataValue):

        currentNode = self.root
        locationFound = False

        # Check to see if tree is Empty
        if currentNode == None:
            self.root = Node(dataValue)
            print "Added Root Node %s" % str(dataValue)
            locationFound = True
        else:
            while locationFound == False:
                if dataValue > currentNode.data:
                    if currentNode.right == None:
                        currentNode.right = Node(dataValue)
                        print "Added Node %s" % str(dataValue)
                        locationFound = True
                    else:
                        currentNode = currentNode.right
                elif dataValue < currentNode.data:
                    if currentNode.left == None:
                        currentNode.left = Node(dataValue)
                        print "Added Node %s" % str(dataValue)
                        locationFound = True
                    else:
                        currentNode = currentNode.left

        # Increment Counter
        self.count += 1
Пример #2
0
def generateTree(training, gain, objectiveValues, attributes = None):

    usedAttributes = []
    if attributes == None:
        remainingAttributes = copy(attributeNames)
    else:
        remainingAttributes = copy(attributes)
    root = Node(None)
    root.height = 0
    F = deque([root])
    E = [root]
    while len(F) > 0:
        curr = F.popleft()
        
        auxData = training
        for filter in curr.filters:
            auxData = auxData[ auxData[filter[0]] == filter[1]]
        gainsPerAttribute = {}
        if len(remainingAttributes) > 0:
            for aName in remainingAttributes:
                gainsPerAttribute[aName] = gain(training, [], obj, aName)
            currAttName = max(gainsPerAttribute, key=lambda key: gainsPerAttribute[key])
            remainingAttributes = [att for att in remainingAttributes if att != currAttName]
            curr.value = currAttName
            attValues = list(data[currAttName].unique())
            # print(auxData)
            for v in attValues:
                child = Node(curr)
                child.height = curr.height + 1
                child.filters = copy(curr.filters)
                child.filters.append([currAttName, v])
                childAuxData = auxData[auxData[currAttName] == v]
                # En este caso el subset para este nodo tiene ejemplos,
                # es decir, hay casos que cumplen con este filtro
                # más específico
                if len(childAuxData) != 0:
                    ## Acá me fijo si todos los elementos del conjunto 
                    ## que me queda agregándole el filtro de este nodo
                    ## son del mismo valor del objetivo -> el nodo es hoja
                    leaf = False
                    for ov in objectiveValues:
                        if len(childAuxData[childAuxData[obj] == ov]) == len(childAuxData):
                            leaf = True
                            break
                    if leaf:
                        child.value = ov
                    else:
                        F.append(child)
                # En este caso, no hay nodos que cumplan con este nuevo filtro:
                # se crea una hoja cuyo valor es el más frecuente del atributo
                # objetivo en el conjunto del padre
                else:
                    child.value = findMostFrequentObjectiveValue(auxData, obj, objectiveValues)
                
                curr.children[v] = child
                E.append(child)
        # Si no me quedan atributos, los nodos que quedan son hojas
        else:
            curr.value = findMostFrequentObjectiveValue(auxData, obj, objectiveValues)
    return root
Пример #3
0
def MinimalTree(A: List[int]) -> Node:
    if not A:
        return None

    lenA = len(A)

    if lenA == 1:
        return Node(A[0])

    root = Node(A[lenA // 2])
    root.left = MinimalTree(A[: lenA // 2])
    root.right = MinimalTree(A[lenA // 2 :])

    return root
Пример #4
0
def maketree3(preorder, postorder):
    if len(preorder) == 0 or len(postorder) == 0:
        return None
    rootVal = postorder[-1]
    rChildIndex = preorder.index(postorder[-2])
    lpreorder = filter(lambda x: preorder.index(x) < rChildIndex, preorder)
    rpreorder = filter(lambda x: preorder.index(x) >= rChildIndex, preorder)
    lpostorder = filter(lambda x: x in lpreorder, postorder)
    rpostorder = filter(lambda x: x in rpreorder, postorder)

    root = Node(rootVal)
    root.left = maketree3(lpreorder, lpostorder)
    root.right = maketree3(rpreorder, rpostorder)

    return root
Пример #5
0
def maketree2(postorder, inorder):
    if len(postorder) == 0 or len(inorder) == 0:
        return None

    rootVal = postorder[-1]
    rootIndex = inorder.index(rootVal)
    linorder = filter(lambda x: inorder.index(x) < rootIndex, inorder)
    rinorder = filter(lambda x: inorder.index(x) > rootIndex, inorder)
    lpostorder = filter(lambda x: x in linorder, postorder)
    rpostorder = filter(lambda x: x in rinorder, postorder)

    root = Node(rootVal)
    root.left = maketree2(lpostorder, linorder)
    root.right = maketree2(rpostorder, rinorder)

    return root
Пример #6
0
def findNextStates(curr, goalSquares, maze, E, cornerSense=False):
    boxes = curr.state.boxes
    user = curr.state.user
    newStates = []
    for dir in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
        moveCost = 0
        boxMoved = ()
        newBoxes = boxes
        hasChanged = False
        tdir = (user[0] + dir[0], user[1] + dir[1])
        t = maze[tdir[0]][tdir[1]]
        if tdir in boxes:
            ttdir = (user[0] + 2 * dir[0], user[1] + 2 * dir[1])
            tt = maze[ttdir[0]][ttdir[1]]
            moveCost += 1
            if tt == ' ' or tt == '.' and ttdir not in boxes:
                hasChanged = True
                boxMoved = tdir
        elif t == ' ' or t == '.':
            moveCost += 2
            hasChanged = True
        if hasChanged:
            newUser = (user[0] + dir[0], user[1] + dir[1])
            if len(boxMoved) > 0:
                newBoxes = [b for b in boxes if b != tdir]
                newBoxes.append(ttdir)
            toBeAdded = State(newBoxes, newUser)
            if not toBeAdded in E:
                E.append(toBeAdded)
                if not cornerSense or not toBeAdded.hasUnmovableBox(
                        maze, goalSquares):
                    newStates.append(
                        Node(p=curr, state=toBeAdded, g=curr.g + moveCost))
    return newStates
Пример #7
0
def createDecisionTree(traindata, treenode, depth):
    if (len(traindata) == 1):
        getTreeNodeVal(traindata, treenode)

    elif (treenode.depth <= depth):
        (left, right, treenode_update) = splitData(traindata, treenode)

        treenode_update.left = Node()
        treenode_update.left.depth = treenode_update.depth
        createDecisionTree(left, treenode_update.left, depth)

        treenode_update.right = Node()
        treenode_update.right.depth = treenode_update.depth
        createDecisionTree(right, treenode_update.right, depth)
    elif (len(traindata) > 0):
        getTreeNodeVal(traindata, treenode)
def trainAndTestData(splitedTrainData, splitedTestData, depth):

    decisiontree = Node()
    createDecisionTree(splitedTrainData, decisiontree, depth)

    summse = 0.0
    for i in range(0, len(splitedTestData)):
        label = getLableFromTestData(splitedTestData[i], decisiontree)
        labelT = float(splitedTestData[i][57])
        diff = label - labelT
        summse = summse + diff * diff
    print "MSE = " + str(summse / len(splitedTestData))
Пример #9
0
def trainAndTestData(trainData, testData):
    traindata = getDataBySplitSpace("Data//housing_train.txt", 14)
    regressiontree = Node()
    Tree_Depth = 3
    createRegressionTree(traindata, regressiontree, Tree_Depth)

    summse = 0.0
    for i in range(0, len(testData)):
        diff = getLableFromTestData(testData[i],
                                    regressiontree) - testData[i][13]
        summse = summse + diff * diff
    return summse / len(testData)
Пример #10
0
    def addRecursively(self, currentNode, dataValue):

        # Check to see if tree is Empty
        if currentNode == None:
            self.root = Node(dataValue)
            print "Added Root Node %s" % str(dataValue)
            self.count += 1
        # Add a node onto the tree
        else:
            if dataValue > currentNode.data:
                if currentNode.right == None:
                    currentNode.right = Node(dataValue)
                    print "Added Node %s" % str(dataValue)
                    self.count += 1
                else:
                    self.addRecursively(currentNode.right, dataValue)
            elif dataValue < currentNode.data:
                if currentNode.left == None:
                    currentNode.left = Node(dataValue)
                    print "Added Node %s" % str(dataValue)
                    self.count += 1
                else:
                    self.addRecursively(currentNode.left, dataValue)
Пример #11
0
 def insert(self, data):
     node = Node(data)
     if self.root is None:
         self.root = node
     else:
         current = self.root
         parent = None
         while True:
             parent = current  # remember parent node
             if node.data <= current.data:
                 current = current.left
                 if current is None:
                     parent.left = node
                     return
             else:
                 current = current.right
                 if current is None:
                     parent.right = node
                     return
Пример #12
0
	def solveMCTS(self,start,costmap,goalLoc,MC):
		self.costmap = costmap
		#Worst case scenario: decrease exploration, increase tree depth (may not work)
		self.gamma = .9; 

		#Number of Actions
		self.numActions = self.model.acts;

		#Max solver time per action
		self.maxTime = 0.5; #max time I spend searching

		#Exploration Constant
		self.c = 1;  
		self.goal = 20*goalLoc[0] + goalLoc[1]

		#print ('Goal:' , convertToGridCoords(self.goal,20,20))
		#Starting state
		state = 20*start[0] + start[1]; 

		#print ('Start:' , convertToGridCoords(state,20,20))
		#Set max depth
		maxDepth = 100; #breadth vs depth


		state_list = []
		reward = 0
		result = 0;
		count = 0; 
		goal = convertToGridCoords(self.goal,self.model.width,self.model.height)
		s = convertToGridCoords(state,self.model.width,self.model.height)
		d2g = self.costmap[goal[0]][goal[1]][s[0]][s[1]] #double flip this is correct
		if d2g < 2:
			self.c = 0
			maxDepth = 1


		while(count < 60):

			h = Node(); 
			act = self.search(state,h,maxDepth); #fake
			
			return act+1
Пример #13
0
 def __init__(self, mapPath):
     self.Tr = Node()
     self.Q0 = []
     self.goalSquares = []
     file = open(mapPath)
     lines = file.readlines()
     boxes = []
     for num, line in enumerate(lines):
         line = list(line.rstrip("\n"))
         try:
             j = line.index("@")
             user = (num, j)
             line[j] = ' '
         except ValueError:
             self
         try:
             js = [ (num, i) for i, elem in enumerate(line) if elem == '.']
             self.goalSquares.extend(js)
         except ValueError:
             self
         try:
             js = [ (num, i) for i, elem in enumerate(line) if elem == '$']
             boxes.extend(js)
             for j in js:
                 line[j[1]] = ' '
         except ValueError:
             self
         try:
             js = [ (num, i) for i, elem in enumerate(line) if elem == '*']
             boxes.extend(js)
             self.goalSquares.extend(js)
             for j in js:
                 line[j[1]] = '.'
         except ValueError:
             self
         self.Q0.append(line)
     self.Tr.state = State(boxes, user)
     file.close()
Пример #14
0
    def action_callback(self, msg):

        if (msg.data == 1):

            rospy.sleep(3)
            print("Publishing goal: [{},{}]".format(self.nextGoal[0],
                                                    self.nextGoal[1]))
            self.goal_pub.publish([int(self.nextGoal[0])],
                                  [int(self.nextGoal[1])], 0)
            print("Goal Published")
            h = Node()
            act, info = self.solver.search(self.sSet,
                                           h,
                                           depth=self.solver.maxDepth,
                                           maxTime=min(self.curDecTime,
                                                       self.solver.maxTime),
                                           inform=True)
            self.latestAction = act
            self.solver.buildActionSet(self.trueS[7])
            initializing = False

            # Catches NAN drone state by resetting with current node location
            for s in self.sSet:
                if np.isnan(s[0]) or np.isnan(s[0]):
                    s[0] = s[-1].loc[0]
                    s[1] = s[-1].loc[0]
                    initializing = True
            if initializing:  #print statement to show that a NaN was caught and reinitialized
                print('Drone State Re-initialized')

            # print('Action Set',self.solver.actionSet)
            try:  # Forbidden nodes probably leads to shorter action set sequences, requiring this try-catch statement
                if (self.solver.actionSet[act][1][0] is not None):
                    self.lastAct = act
                    message = pull(
                        "Is the target {} of {}".format(
                            self.solver.actionSet[act][1][1],
                            self.solver.actionSet[act][1][0].name),
                        self.msg_count)
                    self.question_pub.publish(message)
                    self.msg_count += 1
            except:
                print('Solver List Shortened')
                # print('Action Set',self.solver.actionSet)
                print('Action number', act, 'List Size',
                      len(self.solver.actionSet))
                act = len(self.solver.actionSet
                          ) - 1  # Taking the last action in the list

            self.curDecTime = dist(
                self.trueS,
                self.solver.actionSet[act][0].loc) / (self.solver.agentSpeed)
            self.totalTime += self.curDecTime

            self.step += 1

            for i in range(0, int(np.ceil(self.curDecTime))):
                self.sSet = self.solver.dynamicsUpdate(
                    self.sSet, self.solver.actionSet[act])

            try:

                #self.sSet = self.solver.measurementUpdate_time(self.sSet,self.solver.actionSet[act], 'Null Null');
                self.trueS[7] = self.solver.actionSet[act][0]
                self.trueS[0] = self.trueS[7].loc[0]
                self.trueS[1] = self.trueS[7].loc[1]

                self.nextGoal = [
                    self.trueS[7].loc[0] - self.offset_x,
                    1000 - self.trueS[7].loc[1] - self.offset_y
                ]
                print("Action Ready")
                print("")
            except Exception as e:
                print("Belief Update Issue Raised")
                pass
Пример #15
0
    d1 = []
    # to store distance of n2 from lca
    d2 = []
    # if lca exist
    if lca:
        # distance of n1 from lca
        findLevel(lca, n1, d1, 0)
        # distance of n2 from lca
        findLevel(lca, n2, d2, 0)
        return d1[0] + d2[0]
    else:
        return -1


# Driver program to test above function
root = Node(1)
root.left = Node(2)
root.right = Node(3)
root.left.left = Node(4)
root.left.right = Node(5)
root.right.left = Node(6)
root.right.right = Node(7)
root.right.left.right = Node(8)

root.display()
# root.prettyPrint()

print("Dist(4,5) = ", findDistance(root, 4, 5))
print("Dist(4,6) = ", findDistance(root, 4, 6))
print("Dist(3,4) = ", findDistance(root, 3, 4))
print("Dist(2,4) = ", findDistance(root, 2, 4))
Пример #16
0
from getDataBySplitSpace import getDataBySplitSpace
from getLableFromTestData import getLableFromTestData
from createRegressionTree import createRegressionTree
from treeNode import Node
from splitDataTenFold import splitDataTenFold
from trainAndTestData import trainAndTestData

####################################################
##################train data#########################
####################################################
traindata = getDataBySplitSpace("Data//housing_train.txt", 14)
regressiontree = Node()
Tree_Depth = 6
createRegressionTree(traindata, regressiontree, Tree_Depth)

####################################################
##################test data#########################
####################################################
testdata = getDataBySplitSpace("Data//housing_test.txt", 14)

summse = 0.0
for i in range(0, len(testdata)):
    diff = getLableFromTestData(testdata[i], regressiontree) - testdata[i][13]
    summse = summse + diff * diff
print "MSE = " + str(summse / len(testdata))

#tree depth     mse
#    0         34.51
#    1         25.57
#    2         23.33
#    3         18.94
Пример #17
0
from treeNode import Node
from printBinaryTree import printTree


def postorderPrint(root):
    if root:
        postorderPrint(root.left)
        postorderPrint(root.right)
        print(root.val)


if __name__ == '__main__':
    ROOT = Node(1)
    ROOT.left = Node(2)
    ROOT.right = Node(3)
    ROOT.left.left = Node(4)
    ROOT.left.right = Node(5)
    ROOT.right.left = Node(6)
    ROOT.right.right = Node(7)

    #printTree(ROOT)
    postorderPrint(ROOT)
Пример #18
0
def runSims(sims = 10,steps = 10,verbosity = 2,simIdent = 'Test'):

	#set up data collection
	dataPackage = {'Meta':{'NumActs':numActs,'maxDepth':maxDepth,'c':c,'maxTreeQueries':maxTreeQueries,'maxTime':maxTime,'gamma':gamma,'numObs':numObs,'problemName':problemName,'agentSpeed':agentSpeed},'Data':[]}
	for i in range(0,sims):
		dataPackage['Data'].append({'Beliefs':[],'ModeBels':[],'States':[],'Actions':[],'Observations':[],'Rewards':[],'TreeInfo':[]}); 

	if(verbosity >= 1):
		print("Starting Data Collection Run: {}".format(simIdent)); 
		print("Running {} simulations of {} steps each".format(sims,steps))
	#run individual sims
	for count in range(0,sims):
		if(verbosity >= 2):
			print("Simulation: {} of {}".format(count+1,sims)); 
		


		#Make Problem
		h = Node(); 
		solver = POMCP(); 

		#Initialize Belief and State
		network = readInNetwork('../common/flyovertonNetwork.yaml')
		setNetworkNodes(network); 
		target,curs,goals = populatePoints(network,maxTreeQueries); 
		pickInd = np.random.randint(0,len(target)); 
		trueS = [np.random.random()*8,np.random.random()*8,target[pickInd][0],target[pickInd][1],curs[pickInd],goals[pickInd],0]; 

		sSet = []; 
		for i in range(0,len(target)):
			sSet.append([trueS[0],trueS[1],target[i][0],target[i][1],curs[i],goals[i],0]); 

		#For storage purposes, only the mean and sd of the belief are kept
		#dataPackage['Data'][count]['Beliefs'].append(sSet);
		mean = [sum([sSet[i][2] for i in range(0,len(sSet))])/len(sSet),sum([sSet[i][3] for i in range(0,len(sSet))])/len(sSet)]; 
		
		tmpBel = np.array(sSet); 
		mean = [np.mean(tmpBel[:,2]),np.mean(tmpBel[:,3])];
		sd =  [np.std(tmpBel[:,2]),np.std(tmpBel[:,3])];

		dataPackage['Data'][count]['Beliefs'].append([mean,sd]); 
		dataPackage['Data'][count]['States'].append(trueS); 
		if(verbosity >= 4):
			fig,ax1 = plt.subplots(); 
		for step in range(0,steps):
			if(verbosity>=3):
				print("Step: {}".format(step));  
			if(verbosity >=4):
				fig,ax1 = displayNetworkMap('../common/flyovertonNetwork.yaml',fig,ax1,False,redraw=True);


			act,info = solver.search(sSet,h,depth = min(maxDepth,steps-step+1),inform=True);


			trueS = generate_s(trueS,act); 
			r = generate_r(trueS,act);
			o = generate_o(trueS,act); 

			tmpHAct = h.getChildByID(act); 
			tmpHObs = tmpHAct.getChildByID(o); 

			if(tmpHObs != -1 and len(tmpHObs.data) > 0):
				h = tmpHObs; 
				#sSet = solver.resampleNode(h); 
			else:
				h = tmpHAct[0]; 
				#print("Error: Child Node Not Found!!!"); 

			sSet = propogateAndMeasure(sSet,act,o); 
			sSet = solver.resampleSet(sSet); 

			tmpBel = np.array(sSet); 
			#print(len(tmpBel)); 
			mean = [np.mean(tmpBel[:,2]),np.mean(tmpBel[:,3])];
			sd =  [np.std(tmpBel[:,2]),np.std(tmpBel[:,3])];

			modeBels = [len(np.where(tmpBel[:,6] == 0)[0]), len(np.where(tmpBel[:,6] == 1)[0])]; 


			################################################
			if(verbosity >= 4):
				ax2 = fig.add_subplot(111,label='belief'); 
				sp=[tmpBel[:,2],tmpBel[:,3]];
				
				#ax2.hist2d(sp[0],sp[1],bins=40,range=[[-.2,8.2],[-.2,8.2]],cmin=1,cmap='Reds',zorder=2);
				ax2.scatter(sp[0],sp[1],c='k',zorder=2);
				#ax2.scatter(sp[0],sp[1],c='k',zorder=2);
				ax2.set_xlim([-0.2,8.2]); 
				ax2.set_ylim([-0.2,8.2]);

				ax2.scatter(trueS[0],trueS[1],c=[0,0,1],zorder = 3); 
				ax2.scatter(trueS[2],trueS[3],c=[1,0,0],zorder = 3); 
				#ax2.arrow(trueS[0],trueS[1],trueS[0]-trueS[0],trueS[1]-trueS[1],edgecolor=[0,0,1],head_width = 0.25,facecolor =[0,0,.5],zorder=3); 
				#ax2.arrow(trueS[2],trueS[3],trueS[2]-trueS[2],trueS[3]-trueS[3],edgecolor=[1,0,0],head_width = 0.25,facecolor = [.5,0,0],zorder=3); 
				ax2.add_patch(Circle([trueS[0],trueS[1]],1,alpha=0.25,edgecolor='b'))
				ax2.set_aspect('equal'); 
				ax1.set_aspect('equal');
				ax1.set_xlim([-0.2,8.2]); 
				ax1.set_ylim([-0.2,8.2]); 
				plt.axis('off')
				ax1.axis('off')
				ax2.axis('off')
				plt.axis('off')
				#plt.colorbar() 
				plt.pause(0.01);
				ax2.remove();
			################################################

			dataPackage['Data'][count]['Beliefs'].append([mean,sd]); 
			dataPackage['Data'][count]['States'].append(trueS); 
			dataPackage['Data'][count]['Actions'].append(act); 
			dataPackage['Data'][count]['Observations'].append(o); 
			dataPackage['Data'][count]['Rewards'].append(r); 
			dataPackage['Data'][count]['TreeInfo'].append(info);
			dataPackage['Data'][count]['ModeBels'].append(modeBels); 
			
			if(isTerminal(trueS,act)):
				#print(trueS); 
				if(verbosity >= 2):
					print("Captured after: {} steps".format(step)); 
				break; 


		print("Capture Time: {}".format(len(dataPackage['Data'][count]['Rewards'])-1));
		print("Average Capture Time: {}".format(sum([len(dataPackage['Data'][i]['Rewards'])-1 for i in range(0,count+1)])/(count+1)));
		print(""); 
		np.save('../../data/dataGridNaive_E2_{}'.format(simIdent),dataPackage)
Пример #19
0
def simForward(steps = 10):
	#Make problem
	h = Node(); 
	solver = POMCP(); 

	#make belief

	network = readInNetwork('../common/flyovertonNetwork.yaml')
	setNetworkNodes(network); 
	target,curs,goals = populatePoints(network,maxTreeQueries); 
	pickInd = np.random.randint(0,len(target)); 
	trueS = [np.random.random()*8,np.random.random()*8,target[pickInd][0],target[pickInd][1],curs[pickInd],goals[pickInd],0]; 

	sSet = []; 
	for i in range(0,len(target)):
		sSet.append([trueS[0],trueS[1],target[i][0],target[i][1],curs[i],goals[i],0]); 

	# trueX = np.random.random()*10; 
	# trueY = np.random.random()*10; 
	# sSet = []; 
	# for i in range(0,maxTreeQueries):
	# 	sSet.append([trueX,trueY,np.random.random()*10,np.random.random()*10,np.random.choice([0,1,2]),np.random.choice([-1,1])]); 

	# trueS = sSet[np.random.choice([0,len(sSet)-1])]; 



	fig,ax1 = plt.subplots(); 
	plotFudge = 20; 
	allPrevs = np.zeros(shape=(steps,6)).tolist(); 
	allRewards = []; 
	allMeans = np.zeros(shape = (steps,2)); 
	allVars = np.zeros(shape = (steps,2)); 

	#fig,ax1 = displayNetworkMap('flyovertonNetwork.yaml',fig=fig,ax=ax1,vis=False); 
	#plt.show()

	#get action
	for step in range(0,steps):
		fig,ax1 = displayNetworkMap('../common/flyovertonNetwork.yaml',fig,ax1,False,redraw=True);

		allPrevs[step] = trueS; 
		act = solver.search(sSet,h,False);

		r = generate_r(trueS,act);  
		trueS = generate_s(trueS,act); 
		o = generate_o(trueS,act); 

		allRewards.append(r); 



		tmpHAct = h.getChildByID(act); 
		tmpHObs = tmpHAct.getChildByID(o); 
		#tmpBel = np.array(h.data); 
		

		if(tmpHObs != -1 and len(tmpHObs.data) > 0):
			h = tmpHObs; 
			#sSet = solver.resampleNode(h); 
		else:
			#h = np.random.choice(h.children);
			# print(h); 
			# print("State: {}".format(trueS));
			# print("Action: {}".format(act)); 
			# print("Observation: {}".format(o)); 
			# raise("Error: Child Node not Found!!!")
			h = tmpHAct[0]; 
			print("Error: Child Node Not Found!!!"); 
			
		sSet = propogateAndMeasure(sSet,act,o); 
		sSet = solver.resampleSet(sSet); 

		tmpBel = np.array(sSet); 
		
		allMeans[step] = [np.mean(tmpBel[:,2]),np.mean(tmpBel[:,3])];
		allVars[step] =  [np.std(tmpBel[:,2]),np.std(tmpBel[:,3])]; 

		ax2 = fig.add_subplot(111,label='belief'); 
		sp=[tmpBel[:,2],tmpBel[:,3]];
		
		#ax2.hist2d(sp[0],sp[1],bins=40,range=[[-.2,8.2],[-.2,8.2]],cmin=1,cmap='Reds',zorder=2);
		ax2.scatter(sp[0],sp[1],c='k',zorder=2);
		#ax2.scatter(sp[0],sp[1],c='k',zorder=2);
		ax2.set_xlim([-0.2,8.2]); 
		ax2.set_ylim([-0.2,8.2]);

		ax2.scatter(allPrevs[step][0],allPrevs[step][1],c=[0,0,1],zorder = 3); 
		ax2.scatter(allPrevs[step][2],allPrevs[step][3],c=[1,0,0],zorder = 3); 
		ax2.arrow(allPrevs[step][0],allPrevs[step][1],trueS[0]-allPrevs[step][0],trueS[1]-allPrevs[step][1],edgecolor=[0,0,1],head_width = 0.25,facecolor =[0,0,.5],zorder=3); 
		ax2.arrow(allPrevs[step][2],allPrevs[step][3],trueS[2]-allPrevs[step][2],trueS[3]-allPrevs[step][3],edgecolor=[1,0,0],head_width = 0.25,facecolor = [.5,0,0],zorder=3); 
		
		ax1.set_xlim([-0.2,8.2]); 
		ax1.set_ylim([-0.2,8.2]); 
		plt.axis('off')
		ax1.axis('off')
		ax2.axis('off')
		plt.axis('off')
		#plt.colorbar() 
		plt.pause(0.01);
		#plt.show();
		#ax2.clear(); 
		

		print("Step: {} of {}".format(step+1,steps))
		print("State: {}".format(trueS));
		print("Action: {}".format(act)); 
		print("Observation: {}".format(o)); 
		#print("Distance: {0:.2f}".format(dist(trueS)))
		print("Ac Reward: {}".format(sum(allRewards))); 
		print("Belief Mean: {0:.2f},{0:.2f}".format(np.mean(tmpBel[:,2]),np.mean(tmpBel[:,3]))); 
		print("Belief Length: {}".format(len(tmpBel)))
		print(""); 
		#print(info)

		if(isTerminal(trueS,act)):
			print("Captured after: {} steps".format(step)); 
			break; 
		ax2.remove();


		#ax.scatter(trueS[0],trueS[1],c=[0,0,1,((step+plotFudge)/(steps+plotFudge))]); 
		#ax.scatter(trueS[2],trueS[3],c=[0,1,0,((step+plotFudge)/(steps+plotFudge))]); 
		# ax.scatter(tmpBel[:,2],tmpBel[:,3],c=[1,0,0,0.25],marker='*',s=2)
		# ax.scatter(allPrevs[step][0],allPrevs[step][1],c=[0,0,1]); 
		# ax.scatter(allPrevs[step][2],allPrevs[step][3],c=[0,1,0]); 
		# ax.arrow(allPrevs[step][0],allPrevs[step][1],trueS[0]-allPrevs[step][0],trueS[1]-allPrevs[step][1],edgecolor=[0,0,1],head_width = 0.25,facecolor =[0,0,.5]); 
		# ax.arrow(allPrevs[step][2],allPrevs[step][3],trueS[2]-allPrevs[step][2],trueS[3]-allPrevs[step][3],edgecolor=[0,1,0],head_width = 0.25,facecolor = [0,.5,0]); 
		

		# allC = np.zeros(shape=(step,4)); 
		# allC[:,2] = 1; 
		# for i in range(0,len(allC)):
		# 	allC[i,3] = .6*(i/len(allC)) 

		# ax.scatter(allPrevs[:,0],allPrevs[:,1],c=allC)
		 
		
	# 	plt.xlim([-0.5,10.5]); 
	# 	plt.ylim([-0.5,10.5]); 
	# 	plt.pause(0.001)

	# 	plt.cla();

	# plt.clf(); 
	#plt.pause(2);

	#print("Final Accumlated Reward after {} steps: {}".format(steps,sum(allRewards))); 
	fig,axarr = plt.subplots(2); 
	x = range(0,steps); 
	allPrevs = np.array(allPrevs); 

	axarr[0].plot(allMeans[:,0],c='g');
	axarr[0].plot(allMeans[:,0] + 2*allVars[:,0],c='g',linestyle='--')
	axarr[0].plot(allMeans[:,0] - 2*allVars[:,0],c='g',linestyle='--')
	axarr[0].plot(allPrevs[:,2],c='k',linestyle='--')
	axarr[0].fill_between(x,allMeans[:,0] - 2*allVars[:,0],allMeans[:,0] + 2*allVars[:,0],alpha=0.25,color='g')
	axarr[0].set_ylim([-0.5,10.5]); 
	axarr[0].set_ylabel('North Estimate')

	axarr[1].plot(allMeans[:,1],c='g');
	axarr[1].plot(allMeans[:,1] + 2*allVars[:,1],c='g',linestyle='--')
	axarr[1].plot(allMeans[:,1] - 2*allVars[:,1],c='g',linestyle='--')
	axarr[1].plot(allPrevs[:,3],c='k',linestyle='--')
	axarr[1].fill_between(x,allMeans[:,1] - 2*allVars[:,1],allMeans[:,1] + 2*allVars[:,1],alpha=0.25,color='g')
	axarr[1].set_ylim([-0.5,10.5]); 
	axarr[1].set_ylabel('East Estimate')
	fig.suptitle("Estimates with 2 sigma bounds when caught at: {}".format(step)); 





	plt.show()
Пример #20
0
#simple binary tree
from treeNode import Node

if __name__ == "__main__":
    ROOT = Node(1)
    ROOT.left = Node(2)
    ROOT.right = Node(3)

    print(ROOT.val)
    print(ROOT.left.val)
    print(ROOT.right.val)
Пример #21
0
def simulate(verbosity=0):

    # Initialize network
    # ------------------------------------------------------
    network = readInNetwork('../yaml/flyovertonShift.yaml')
    h = Node()
    solver = POMCP('graphSpec')

    maxFlightTime = 600  #10 minutes
    human_sketch_chance = 1 / 60
    #about once a minute
    pmean = 3
    #poisson mean
    amult = 10
    #area multiplier
    #human_sketch_chance = 0;

    # Initialize belief and state
    # ------------------------------------------------------
    target, curs, goals = populatePoints(network, solver.sampleCount)
    pickInd = np.random.randint(0, len(target))
    trueNode = np.random.choice(network)
    solver.buildActionSet(trueNode)

    trueS = [
        trueNode.loc[0], trueNode.loc[1], target[pickInd][0],
        target[pickInd][1], curs[pickInd], goals[pickInd], 0, trueNode
    ]

    sSet = []
    for i in range(0, len(target)):
        sSet.append([
            trueS[0], trueS[1], target[i][0], target[i][1], curs[i], goals[i],
            0, trueS[7]
        ])

    fig, ax = plt.subplots()

    # DEBUG: Add initial sketch
    # ------------------------------------------------------
    params = {
        'centroid': [500, 500],
        'dist_nom': 50,
        'angle_noise': .3,
        'dist_noise': .25,
        'pois_mean': 4,
        'area_multiplier': 5,
        'name': "Test",
        'steepness': 20,
        'points': None
    }
    ske = Sketch(params)
    solver.addSketch(trueS[7], ske)

    # Set up sketches
    # ------------------------------------------------------
    with open("../yaml/landmarks.yaml", 'r') as stream:
        fi = yaml.safe_load(stream)

    params = {
        'centroid': [4, 5],
        'dist_nom': 2,
        'dist_noise': .25,
        'angle_noise': .2,
        'pois_mean': pmean,
        'area_multiplier': amult,
        'name': "Test",
        'steepness': 7
    }
    allSketches = []
    #seedCount = 0
    for k, v in fi['Landmarks'].items():
        #for k in fi.keys:
        v = fi['Landmarks'][k]
        params['name'] = k
        params['dist_nom'] = v['radius']
        params['centroid'] = v['loc']
        params['dist_noise'] = v['radius'] / 4
        params['points'] = None
        allSketches.append(Sketch(params))
        #seedCount += 1
    np.random.shuffle(allSketches)
    sketchQueue = deque(allSketches)

    sketchTimes = []
    expCount = 0
    for i in range(0, len(sketchQueue)):
        expCount += expon.rvs(scale=1 / human_sketch_chance)
        sketchTimes.append(expCount)

    # print(sketchTimes);
    # sketchTimes.pop(0);
    # print(sketchTimes);

    # Set up data collection
    # ----------------------------------------------------------
    # Note: Beliefs should only be s[2:4]
    # to capture target state beliefs and s[6] to capture mode
    data = {
        'States': [],
        'Actions': [],
        'Human_Obs': [],
        'Drone_Obs': [],
        'Beliefs': [],
        'ModeBels': [],
        'TotalTime': 0,
        'DecisionTimes': [],
        'GivenTimes': [],
        'Sketches': []
    }
    data['maxDepth'] = solver.maxDepth
    data['c'] = solver.c
    data['maxTreeQueries'] = solver.maxTreeQueries
    data['maxTime'] = solver.maxTime
    data['gamma'] = solver.gamma
    data['agentSpeed'] = solver.agentSpeed
    data['sampleCount'] = solver.sampleCount
    data['human_class_thresh'] = solver.human_class_thresh
    data['human_accuracy'] = solver.human_accuracy
    data['capture_length'] = solver.capture_length
    data['detect_length'] = solver.detect_length
    data['drone_falseNeg'] = solver.drone_falseNeg
    data['drone_falsePos'] = solver.drone_falsePos
    data['targetSpeed'] = solver.targetSpeed
    data['targetDev'] = solver.targetDev
    data['offRoadSpeed'] = solver.offRoadSpeed
    data['offRoadDev'] = solver.offRoadDev
    data['leaveRoadChance'] = solver.leaveRoadChance
    data['human_availability'] = solver.human_availability
    data['maxFlightTime'] = maxFlightTime
    data['human_sketch_chance'] = human_sketch_chance
    data['assumed_availability'] = solver.assumed_availability
    data['assumed_accuracy'] = solver.assumed_accuracy
    data['Captured'] = False
    data['pois_mean'] = pmean
    data['area_multiplier'] = amult
    #print(data['maxTime']);

    endFlag = False
    totalTime = 0
    step = 0
    curDecTime = 5
    decCounts = 0
    sketchesMade = []
    # Simulate until captured or out of time
    # ------------------------------------------------------
    #for step in range(0, maxSteps):
    newSet = sSet
    while (totalTime < maxFlightTime):
        data['States'].append(trueS)
        bel = np.array(sSet)[:, 2:4]
        modebel = np.array(sSet)[:, 7]

        data['Beliefs'].append(bel)
        data['ModeBels'].append(modebel)

        # POMCP makes decision
        decisionFlag = False
        # -----------------------------------------------------

        if (trueS[0] == trueS[7].loc[0] and trueS[1] == trueS[7].loc[1]):
            if (verbosity > 0):
                print(
                    "Starting step: {} with Decision Time: {:0.2f}s at Total Time: {:0.2f}s"
                    .format(step + 1, min(solver.maxTime, curDecTime),
                            totalTime))
            decCounts = 0
            decisionFlag = True
            #act,info = solver.search(sSet, h, depth=min(solver.maxDepth, maxSteps-step+1), maxTime = min(curDecTime,solver.maxTime),inform=True)
            act, info = solver.search(newSet,
                                      h,
                                      depth=solver.maxDepth,
                                      maxTime=min(curDecTime, solver.maxTime),
                                      inform=True)
            newSet = sSet

            solver.buildActionSet(trueS[7])
            if (verbosity > 1):
                print("Action: {}".format(solver.actionSet[act]))

            try:
                data['Actions'].append(solver.actionSet[act])
            except Exception:
                #print("FAILURE")
                #print(len(solver.actionSet),act);
                #raise;
                act = 0

            #Disable To Blind Plan
            ########################################################
            if (solver.actionSet[act][1][0]
                    is not None):  #If a question is asked regarding a sketch
                # print(solver.actionSet[act],solver.actionSet[act][1],solver.actionSet[act][1][0])
                o = solver.generate_o(trueS, solver.actionSet[act])
                if (verbosity > 1):
                    [o1, o2] = o.split()
                    print("Human observation: {}".format(o2))
                [o1, o2] = o.split()
                data['Human_Obs'].append(o2)
                newSet = np.array(newSet)
                newSet = solver.measurementUpdate(newSet,
                                                  solver.actionSet[act], o)
                newSet = solver.measurementUpdate_time(newSet,
                                                       solver.actionSet[act],
                                                       o)
                newSet = solver.resampleSet(newSet)
                sSet = newSet

            curDecTime = dist(trueS,
                              solver.actionSet[act][0].loc) / solver.agentSpeed
            totalTime += curDecTime

            data['GivenTimes'].append(min(curDecTime, solver.maxTime))
            data['DecisionTimes'].append(totalTime)
            step += 1
            for i in range(0, int(np.ceil(curDecTime))):
                newSet = solver.dynamicsUpdate(newSet, solver.actionSet[act])

        decCounts += 1
        if (decCounts + totalTime > maxFlightTime):
            totalTime += decCounts
            endFlag = True
        fakeAct = [solver.actionSet[act][0], [None, None]]

        #if(verbosity > 1):
        #print("Action: {}".format(solver.actionSet[act]));

        # propagate state
        # ------------------------------------------------------
        #solver.buildActionSet(trueS[7]);
        trueS = solver.generate_s(trueS, solver.actionSet[act])
        r = solver.generate_r(trueS, solver.actionSet[act])

        sSet = solver.dynamicsUpdate(sSet, solver.actionSet[act])
        #o = solver.generate_o(trueS,solver.actionSet[act]);
        o = solver.generate_o(trueS, fakeAct)
        #o = solver.generate_o(trueS,[solver.actionSet[act][0],[solver.actionSet[act][1][0],None]]);
        #o = "Null No"
        [o1, o2] = o.split()
        data['Drone_Obs'].append(o1)
        o = o1 + " Null"
        if ("Captured" in o):
            endFlag = True
            data['Captured'] = True

        if (verbosity > 1):
            print("Drone Observation: {}".format(o1))

        # if question was asked, see if human answered
        # ------------------------------------------------------
        sSet = solver.measurementUpdate(sSet, fakeAct, o)
        sSet = solver.resampleSet(sSet)

        if (decisionFlag):
            tmpHAct = h.getChildByID(act)
            tmpHObs = tmpHAct.getChildByID(o)

            if (tmpHObs != -1 and len(tmpHObs.data) > 0):
                h = tmpHObs
                #sSet = solver.resampleNode(h);
            else:
                h = tmpHAct[0]

        if (verbosity == 2):
            ax.clear()
            sSetNp = np.array(sSet)
            sSetOff = sSetNp[sSetNp[:, 6] == 1]
            sSetOn = sSetNp[sSetNp[:, 6] == 0]
            ax.scatter(sSetOn[:, 2],
                       sSetOn[:, 3],
                       color='magenta',
                       alpha=0.1,
                       edgecolor='none')
            ax.scatter(sSetOff[:, 2],
                       sSetOff[:, 3],
                       color='red',
                       alpha=0.3,
                       edgecolor='none')
            ax.scatter(trueS[0], trueS[1], color='blue', alpha=1, s=100)
            ax.scatter(trueS[2], trueS[3], color='black', alpha=1, s=100)

            # if(solver.actionSet[act][0] is not None):
            #     #theta_options = [180,0,90,270,135,45,315,225]
            #     theta = theta_options[solver.actionSet[act][0]];
            # else:
            #     theta = 90;
            #theta = np.arctan2(trueS[7].loc[1],trueS[7].loc[0])-np.arctan2(trueS[1],trueS[0])
            # theta = np.arctan2(trueS[7].loc[1],trueS[7].loc[0])-np.arctan2(trueS[1],trueS[0])
            # theta = np.degrees(theta);
            theta = computeTheta([trueS[0], trueS[1]], trueS[7].loc)
            #print(theta)

            detect_length = solver.detect_length
            detect_points = [[trueS[0], trueS[1]],
                             [
                                 trueS[0] + detect_length *
                                 math.cos(2 * -0.261799 + math.radians(theta)),
                                 trueS[1] + detect_length *
                                 math.sin(2 * -0.261799 + math.radians(theta))
                             ],
                             [
                                 trueS[0] + detect_length *
                                 math.cos(2 * 0.261799 + math.radians(theta)),
                                 trueS[1] + detect_length *
                                 math.sin(2 * 0.261799 + math.radians(theta))
                             ]]
            detect_poly = Polygon(detect_points)
            x, y = detect_poly.exterior.xy
            ax.plot(x, y, color='blue')

            capture_length = solver.capture_length
            capture_points = [
                [trueS[0], trueS[1]],
                [
                    trueS[0] + capture_length *
                    math.cos(2 * -0.261799 + math.radians(theta)),
                    trueS[1] + capture_length *
                    math.sin(2 * -0.261799 + math.radians(theta))
                ],
                [
                    trueS[0] + capture_length *
                    math.cos(2 * 0.261799 + math.radians(theta)),
                    trueS[1] + capture_length *
                    math.sin(2 * 0.261799 + math.radians(theta))
                ]
            ]
            capture_poly = Polygon(capture_points)

            x, y = capture_poly.exterior.xy
            ax.plot(x, y, color='gold')

            #Show Sketches
            for s in range(0, len(sketchesMade)):
                sketch_Poly = Polygon(sketchesMade[s].points)

                x, y = sketch_Poly.exterior.xy
                ax.plot(x, y, color='green')

                #plt.axis('equal')
            ax.set_xlim([0, 1000])
            ax.set_ylim([0, 1000])

            plt.pause(0.1)

        # check if human volunteered anything
        # ------------------------------------------------------
        # checkVolunteer()

        # check if human sketched anything
        # ------------------------------------------------------
        if (decisionFlag):
            # coin = np.random.random();
            # cTest = expon.cdf(min(curDecTime,solver.maxTime), scale=1/human_sketch_chance)
            # #print(coin,cTest);
            if (len(sketchTimes) > 0 and totalTime > sketchTimes[0]):
                sketchTimes.pop(0)
                #print("Sketch Made: {}".format(ske.name));
                ske = sketchQueue.pop()
                sketchesMade.append(ske)
                solver.addSketch(trueS[7], ske)
                if (verbosity > 1):
                    print("Sketch Made: {}".format(ske.name))
                data['Sketches'].append(ske)

        # save everything
        # ------------------------------------------------------

        # repeat
        # ------------------------------------------------------
        h = Node()

        # repeat
        if (verbosity > 1):
            print("")
        if (endFlag):
            break

    data['TotalTime'] = totalTime

    return data