def draw(screen,cart,force):
"""
Called by the simulation to display the current state of the cart.
"""
# clear screen
screen.fill((0,0,0))
# get the size of the window
wid=gui.dim_window[0]
hei=gui.dim_window[1]
# pendulum length
length=cart.l
scale=hei/length/3.0
# map position onto the screen
x1=wid/2.0+cart.getX()*scale
# if too big/small limit the position
if x1 > wid*.8:
x1 =wid*.8
if x1 < wid*.2:
x1 = wid*.2
# base line for the cart
y1=hei*.6
# angle of pendulum
ang=cart.getAngle()
# x,y of the end of pendulum
x2=x1+scale*math.sin(ang)*length
y2=y1+scale*math.cos(ang)*length
# draw pendulum
col=(255,0,0)
thick=3
gui.draw_line(screen,x1,y1,x2,y2,col,thick)
col=(0,255,0)
gui.fill_circle(screen,x2,y2,12,col)
# draw cart
col=(0,0,255)
thick=20
gui.draw_line(screen,x1-20,y1,x1+20,y1,col,thick)
# display the state of the cart
col=(0,255,255)
state=cart.state
str2=""
str2+= "Phi: %5.2f " % (state[0]-math.pi)
str2+= "dphidt: %5.2f " % state[1]
str2+= "x: %5.2f " % state[2]
str2+= "dxdt: %5.2f " %state[3]
str2+= " force: %5.2f " % force
gui.draw_string(screen,str2,(20,10),col,16)
# copy screen onto the display
gui.blit(screen)
def draw(screen, cart, force):
"""
Called by the simulation to display the current state of the cart.
"""
# clear screen
screen.fill((0, 0, 0))
# get the size of the window
wid = gui.dim_window[0]
hei = gui.dim_window[1]
# pendulum length
length = cart.l
scale = hei / length / 3.0
# map position onto the screen
x1 = wid / 2.0 + cart.getX() * scale
# if too big/small limit the position
if x1 > wid * .8:
x1 = wid * .8
if x1 < wid * .2:
x1 = wid * .2
# base line for the cart
y1 = hei * .6
# angle of pendulum
ang = cart.getAngle()
# x,y of the end of pendulum
x2 = x1 + scale * math.sin(ang) * length
y2 = y1 + scale * math.cos(ang) * length
# draw pendulum
col = (255, 0, 0)
thick = 3
gui.draw_line(screen, x1, y1, x2, y2, col, thick)
col = (0, 255, 0)
gui.fill_circle(screen, x2, y2, 12, col)
# draw cart
col = (0, 0, 255)
thick = 20
gui.draw_line(screen, x1 - 20, y1, x1 + 20, y1, col, thick)
# display the state of the cart
col = (0, 255, 255)
state = cart.state
str2 = ""
str2 += "Phi: %5.2f " % (state[0] - math.pi)
str2 += "dphidt: %5.2f " % state[1]
str2 += "x: %5.2f " % state[2]
str2 += "dxdt: %5.2f " % state[3]
str2 += " force: %5.2f " % force
gui.draw_string(screen, str2, (20, 10), col, 16)
# copy screen onto the display
gui.blit(screen)
sizes=[2,1]
net=None
INIT_ANG=0.3
cnt=0
GOAL=500.0
TOTAL_TIME_MAX=10000
while not gui.check_for_quit(): # loop until user hits escape
# Test for falling over
# if fallen then reset with a random angle
if first or abs(math.pi-cart.getAngle()) > INIT_ANG:
if not first:
state=cart.getState()
fit=time
print cnt,best_fit,fit,tot_time,state,net.weight
if fit > best_fit:
cnt=0
best_fit=fit
best_guess=copy.deepcopy(guess)
cart.setAngle(math.pi+INIT_ANG)
cnt += 1
sizes = [2, 1]
net = None
INIT_ANG = 0.3
cnt = 0
GOAL = 500.0
TOTAL_TIME_MAX = 10000
while not gui.check_for_quit(): # loop until user hits escape
# Test for falling over
# if fallen then reset with a random angle
if first or abs(math.pi - cart.getAngle()) > INIT_ANG:
if not first:
state = cart.getState()
fit = time
print cnt, best_fit, fit, tot_time, state, net.weight
if fit > best_fit:
cnt = 0
best_fit = fit
best_guess = copy.deepcopy(guess)
cart.setAngle(math.pi + INIT_ANG)
cnt += 1
if not inc or best_guess == None or (cnt % 10 == 0):
guess = randomWeights(sizes, 1.0)
GOAL=500.0
TOTAL_TIME_MAX=10000
guess=pool.create()
net=brain.FeedForwardBrain(weight=guess)
# set the initial angle
cart.setAngle(math.pi+INIT_ANG)
time=0.0
tot_time=0.0
while not gui.check_for_quit(): # loop until user hits escape
# Test for falling over
# if fallen then reset with a random angle
if abs(math.pi-cart.getAngle()) > INIT_ANG:
fit=time
pool.add(guess,fit)
print pool
cart.setAngle(math.pi+INIT_ANG)
guess=pool.create()
net.setWeights(guess)
time=0;
input = copy.deepcopy(cart.getState())
input[0]=input[0]-math.pi
out=net.ffwd(input)
force=(out[0]-0.5)*force_scale
while not gui.check_for_quit(): # loop until user hits escape
force=0.0
# check for user key press
keyinput = gui.get_pressed()
if keyinput[gui.keys.K_LEFT]: # push left
force = -1.0
if keyinput[gui.keys.K_RIGHT]: # push right
force = 1.0
# step the car for a single GUI frame
cart.step(force,1.0/frameRate)
################ RESET CODE ##############################
# Test for falling over
# if fallen then reset with a random angle
if abs(math.pi-cart.getAngle()) > math.pi/2:
cart.setAngle(math.pi+0.02*(random.random()-0.5))
############################################################
# draw the cart and display info
draw(screen,cart,force)
# slow the gui down to the given frameRate
gui.tick(frameRate)
while not gui.check_for_quit(): # loop until user hits escape
force = 0.0
# check for user key press
keyinput = gui.get_pressed()
if keyinput[gui.keys.K_LEFT]: # push left
force = -1.0
if keyinput[gui.keys.K_RIGHT]: # push right
force = 1.0
# step the car for a single GUI frame
cart.step(force, 1.0 / frameRate)
################ RESET CODE ##############################
# Test for falling over
# if fallen then reset with a random angle
if abs(math.pi - cart.getAngle()) > math.pi / 2:
cart.setAngle(math.pi + 0.02 * (random.random() - 0.5))
############################################################
# draw the cart and display info
draw(screen, cart, force)
# slow the gui down to the given frameRate
gui.tick(frameRate)