While pondering the question found here: Applications to Video Games, I thought it might be nice to have a visual model for the way localization proceeds.
The following code allows the user to move the robot around the world, sense any of the three colors in that world, and get a visual representation of the robots' belief regarding its position. With a whole lot of imagination I can almost see those homing missiles . . .
Also, this program will allow one to experiment with the implications in the order of move(m) and sense(s). Possible now are (m)(m)(m) or (s)(s)(s) or really any other combination. Also, note that one can see the effects upon the probability distribution when one sends a 'wrong' sense result to a well localized robot.
I will gladly take suggestions as how to make the code more pythonic! I've only been playing with python since the Stanford AI class . . .
So, can you help me improve upon this? Clear explanations for what to replace and why are most helpful!
from Tkinter import *
import math, sys, tkMessageBox
colors = [['blue' , 'blue', 'blue' , 'blue' , 'blue' , 'blue' , 'blue' , ],
['blue' , 'red' , 'green', 'green', 'green', 'red' , 'blue' , ],
['blue' , 'red' , 'red' , 'green', 'red' , 'red' , 'blue' , ],
['blue' , 'red' , 'green', 'red' , 'green', 'red' , 'blue' , ],
['blue' , 'red' , 'green', 'green', 'green', 'red' , 'blue' , ],
['blue' , 'blue', 'blue' , 'blue' , 'blue' , 'blue' , 'blue' , ],]
frameWidth = 1000
frameHeight = 500
rows = len(colors)
cols = len(colors[0])
initProb = ('%.3f' % (1.0/(rows*cols)))
boxWidth = frameWidth / (2 * cols + 4)
boxHeight = frameHeight / (rows + 1)
if boxHeight<boxWidth:
boxWidth = boxHeight
else:
boxHeight = boxWidth
quitYLocation = 0
quitXLocation = cols + 1
def callCheck():
if tkMessageBox.askokcancel("Quit","Really, really quit?"):
root.destroy()
def worldMap():
initRow = 0
for i in range(len(colors)):
initCol = 0
for opt in colors[i]:
c = Label(wMap,
width = boxWidth/8,
height = boxHeight/15,
bg = str(opt),
fg = 'black',
bd = 5,
relief = GROOVE
)
c.grid(row = initRow, column = initCol)
initCol += 1
initCol = 0
initRow += 1
def robotMap(p):
initRow = 0
for i in range(rows):
initCol = cols + 3
for j in range(cols):
c = Label(rMap,
text = str(p[i][j]),
width = boxWidth/8,
height = boxHeight/15,
bg = '#'+hex(int(4095*(1.00 - float(p[i][j]))))[2:]*3,
fg = 'blue',
bd = 5,
relief = GROOVE)
c.grid(row = initRow, column = initCol)
initCol += 1
initCol = cols + 3
initRow += 1
def callSense(Z):
q=[]
global p
for i in range(rows):
for j in range(cols):
hit = (Z == colors[i][j])
q.append(float(p[i][j]) * (hit * sensor_right + (1-hit) * sensor_wrong))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] /s
p = buildP(q)
robotMap(p)
return p
def callMove(U):
q = []
global p
for i in range(rows):
for j in range(cols):
s = pExactMove * float(p[(i - U[0]) %rows][(j - U[1]) %cols])
s = s + pOvershoot * float(p[(i - U[0]-1*U[0])%rows][(j - U[1]-1*U[1])%cols])
s = s + pUndershoot * float(p[(i - U[0]+1*U[0])%rows][(j - U[1]+1*U[1])%cols])
q.append(s)
s = sum(q)
for i in range(len(q)):
q[i] = q[i] /s
p = buildP(q)
robotMap(p)
return p
def buildPRow(q):
global count
for i in range(cols):
row.append('%.3f' % (q[count]))
count+=1
return row
def buildP(q):
global row, count
p = []
row = []
count = 0
for i in range(rows):
p.append(buildPRow(q))
row = []
return p
def buttonMap():
buttonCallQuit.grid (row = quitYLocation+1, column = quitXLocation)
buttonMoveUp.grid (row = quitYLocation, column = quitXLocation)
buttonMoveLeft.grid (row = quitYLocation+1, column = quitXLocation-1)
buttonMoveRight.grid(row = quitYLocation+1, column = quitXLocation+1)
buttonMoveDown.grid (row = quitYLocation+2, column = quitXLocation)
def senseMap():
buttonCallSenseRed.grid (row = quitYLocation, column = quitXLocation-1)
buttonCallSenseGreen.grid(row = quitYLocation, column = quitXLocation)
buttonCallSenseBlue.grid (row = quitYLocation, column = quitXLocation+1)
############################################################################
p = []
q = []
count = 0
sensor_right = 0.75
sensor_wrong = 1.0 - sensor_right
pExactMove = 0.80
pOver = 0.50
pUnder = 1.0 - pOver
pOvershoot = pOver*(1-pExactMove)
pUndershoot = pUnder*(1-pExactMove)
for i in range(rows):
for j in range(cols):
q.append(initProb)
p.append(q)
root = Tk()
root.title("Robot Localization -- Michael T. Smithback")
root.geometry(str(frameWidth)+'x'+str(frameHeight)+'+130+150')
root.protocol("WM_DELETE_WINDOW", callCheck)
root.configure(bg = '#cccffffff')
wMap = LabelFrame(root,
text = 'Known World Map',
bg = '#cccffffff')
bMap = LabelFrame(root,
text = 'Robot Motion Contols',
bg = '#fffbbbddd')
sMap = LabelFrame(root,
text = 'Robot Sense Contols',
bg = '#bbbdddfff')
rMap = LabelFrame(root,
text = 'Robot Belief Map',
bg = '#cccffffff')
buttonCallQuit = Button(bMap,
text = 'Quit',
command = callCheck,
takefocus = "",
overrelief = GROOVE,
fg = 'black',
bg = 'grey',
activebackground = 'red',
width = boxWidth/10,
height = boxHeight/20,
relief = RAISED)
buttonMoveRight = Button(bMap,
text = 'Right',
command = lambda : callMove([0,1]),
takefocus = "",
overrelief = GROOVE,
fg = 'black',
bg = 'grey',
width = boxWidth/10,
height = boxHeight/20,
relief = RAISED)
buttonMoveLeft = Button(bMap,
text = 'Left',
command = lambda : callMove([0,-1]),
takefocus = "",
overrelief = GROOVE,
fg = 'black',
bg = 'grey',
width = boxWidth/10,
height = boxHeight/20,
relief = RAISED)
buttonMoveUp = Button(bMap,
text = 'Up',
command = lambda : callMove([-1,0]),
takefocus = "",
overrelief = GROOVE,
fg = 'black',
bg = 'grey',
width = boxWidth/10,
height = boxHeight/20,
relief = RAISED)
buttonMoveDown = Button(bMap,
text = 'Down',
command = lambda : callMove([1,0]),
takefocus = "",
overrelief = GROOVE,
fg = 'black',
bg = 'grey',
width = boxWidth/10,
height = boxHeight/20,
relief = RAISED)
buttonCallSenseRed = Button(sMap,
text = 'Sense',
command = lambda : callSense('red'),
takefocus = "",
overrelief = GROOVE,
fg = 'black',
bg = 'red',
activebackground = 'red',
width = boxWidth/10,
height = boxHeight/20,
relief = RAISED)
buttonCallSenseGreen = Button(sMap,
text = 'Sense',
command = lambda : callSense('green'),
takefocus = "",
overrelief = GROOVE,
fg = 'black',
bg = 'green',
activebackground = 'green',
width = boxWidth/10,
height = boxHeight/20,
relief = RAISED)
buttonCallSenseBlue = Button(sMap,
text = 'Sense',
command = lambda : callSense('blue'),
takefocus = "",
overrelief = GROOVE,
fg = 'black',
bg = 'blue',
activebackground = 'blue',
width = boxWidth/10,
height = boxHeight/20,
relief = RAISED)
worldMap()
buttonMap()
senseMap()
robotMap(p)
wMap.grid(row = 0, column = 0)
bMap.grid(row = 0, column = 1)
rMap.grid(row = 0, column = 2)
sMap.grid(row = 1, column = 1)
root.mainloop()
########################################################
Screen shot after sensing green five of times, moving down and sensing red five time. One can see the probability build.
Can't you add some screenshot?
@smimic42, upload the screenshot file into the public folder of your DropBox account:
https://www.dropbox.com/
Copy the public link of your screenshot file and use it to include your photo here.
Thanks Juan Carlos!
I now have joined the new century with respect to 'haz Dropbox'.