def test_collision(self):
"""
Collision() determines whether 2 pendulums have collided and if so
adjusts the x component of their velocity accordingly
"""
# Define 2 pendulums and set their angles such that they should collide
# and with arbitrary velocities
angle1 = 0
pivot1 = [0, 0]
p1 = Pendulum(angle1, pivot1[0], pivot1[1])
p1.v_x = 4
angle2 = -0.02 * math.pi
pivot2 = [50, 0]
p2 = Pendulum(angle2, pivot2[0], pivot2[1])
p2.v_x = -2
collide(p1, p2)
# Check their velicties have been swapped
self.assertEqual(p1.v_x, -2)
self.assertEqual(p2.v_x, 4)
# Define a third pendulum angle shuch that it should not collide with p1
angle3 = 0.4 * math.pi
p3 = Pendulum(angle3, pivot2[0], pivot2[1])
p3.v_x = 8
collide(p1, p3)
# Check their velocties have not been swapped
self.assertEqual(p1.v_x, -2)
self.assertEqual(p3.v_x, 8)
def main():
BACKGROUNDC = (255, 255, 255)
WIDTH, HEIGHT = (700, 700)
screen = pygame.display.set_mode((WIDTH, HEIGHT))
pygame.display.set_caption('Pendulum')
clock = pygame.time.Clock()
running = True
pendulum = Pendulum(350, 390, 10, 100, WIDTH/2 , HEIGHT/2, screen)
while running:
clock.tick(50)
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
else:
screen.fill(BACKGROUNDC)
pendulum.display()
pendulum.move()
# in a real game use .update() which is less taxing.
pygame.display.flip()
import pygame, math, time
from Pendulum import Pendulum, dist, onPath, collide
if __name__ == "__main__":
# Define pygame variables
(width, height) = (640, 480)
background_colour = (255, 255, 255)
screen = pygame.display.set_mode((width, height))
pygame.display.set_caption("Pendulum")
running = True
selected = None
# Define the stack of pendulums
pendulum_stack = [
Pendulum(0, 250, 40),
Pendulum(0, 300, 40),
Pendulum(0, 350, 40),
Pendulum(0, 400, 40)
]
# Assign an id to each pendulum
for i, p in enumerate(pendulum_stack):
p.ID = i
# Number of pendulums for reference within the game loop
numPen = len(pendulum_stack)
while running:
# draw background and line pendulums hang from
screen.fill(background_colour)
pygame.draw.line(screen, (0, 0, 255), (120, 40), (520, 40), 3)
ddp = DDPDelay(ddpparams)
uk0 = np.zeros((sys.udim, Horizon))
print 'running ddp...'
ddp.run(x0_model, uk0)
# pr.disable()
# s = StringIO.StringIO()
# ps = pstats.Stats(pr, stream=s)
# ps.sort_stats('time', 'cumulative').print_stats(.5, 'init')
# ps.print_stats()
# ps.print_callers(.5, 'init')
# print >> f, s.getvalue()
#apply control to real system
x0 = np.array([[0.0], [0.0]])
realsys = Pendulum(systeminfo['params'])
print 'running system...'
xtraj_sys = realsys.splitAngle(realsys.runSystem(x0, ddp.uk))
print 'running model...'
xtraj_model = sys.runSystem(x0_model, uk0)
print 'running system with feedback'
xtraj_sys_feedback = realsys.splitAngle(
realsys.runSystemWithFeedback(x0,
ddp.uk,
ddp.K[:, :xdim, 0, :],
xtraj_model[:xdim, :],
feedback_split=True))
print 'plotting...'
fig, axes = plt.subplots(nrows=xdim + 1, ncols=1)
pd.DataFrame(ddp.uk.T).plot(ax=axes[0], legend=False, title='u')
from Pendulum import Pendulum
import math
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import nndynamics_helpers as helpers
print 'hello world'
# pendulum parameters
dt=0.02
L=1.0
params={'dt':dt, 'c':0.01, 'm':0.1, 'g':9.8, 'l':L}
x0=np.array([[0.0],[0.0]])
sys=Pendulum(params)
M=10000
N=50
uscale=100.0
print 'making data...'
#make input
freq=np.logspace(-0.01,1.8,num=M)
xtraj=sys.runSystem(x0,np.zeros((1,N)))
udata=np.zeros((1,N))
for i in range(0,M/4):
u=np.random.uniform(-uscale,uscale,size=(1,N))
xtraj=np.dstack([xtraj,sys.runSystem(x0,u)])
udata=np.dstack([udata,u])
for i in range(0,M):
u=np.sin(np.random.uniform(-math.pi,math.pi)+freq[i]*dt*np.linspace(1.0,N,num=N))*np.random.uniform(-uscale,uscale)
'Simulation_Time': Sim_Time,
'Sampling_Time': Samp_Time,
'System_Model': None,
'Kpid': [None, None, None]
}
Configs_GA = {
'size': 100,
'mutation_rate': 0.05,
'target': Configs_PID['Ref'],
'params_range': [[0, 50], [0, 10], [0, 10]], # [P, I, D]
'count': 10,
'iteration': Iteration,
'max_generation': 50
}
pendulum = Pendulum(mass=2, length=1, fric=0.15)
pid = PID(config=Configs_PID)
genetics = Population(config=Configs_GA)
max_torque = 10 # Nm
genetics.randomize_global_best()
while not genetics.is_finished():
print('Generation #:', genetics.get_generation())
genetics.randomize_local_best()
genetics.clear_genscore()
for j, chromosome in enumerate(genetics.get_all()):
