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 makePlot():
verts = []
codes = []
p1 = Pendulum()
p1.stoppedPendulum(2.0, 45.0)
codes.append(Path.MOVETO)
#begin adding other verts at time
for time in range(1, 1000):
vert = p1.coordAtTime(time)
verts.append(vert)
codes.append(Path.CURVE4)
codes.pop(-1) # there is an extra CURVE4
path = Path(verts, codes)
fig = plt.figure()
ax = fig.add_subplot(111)
patch = patches.PathPatch(path, facecolor='none', lw=2)
ax.add_patch(patch)
#xs, ys = zip(*verts)
#ax.plot(xs, ys, 'x--', lw=2, color='black', ms=10)
ax.set_xlim(-p1.length / 5, p1.length / 5)
ax.set_ylim(4 * p1.length / 5, p1.length)
return plt
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)
'''
Created on May 21, 2012
@author: Alex Kinney
'''
from Pendulum import Pendulum
if __name__ == "__main__":
p = Pendulum()
p.stoppedPendulum(10, 45)
period = p.period()
for time in range(10):
print p.pendulumAngleAtTime(time)
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')
dataprefix = 'pendulum'
#load model
model, systeminfo = helpers.load_model(dataprefix)
modelsys = KerasToDelayedSystemWrapper(model, systeminfo)
dt = systeminfo['dt']
xdelay = systeminfo['xdelay']
xdim = systeminfo['xdim']
udim = systeminfo['udim']
udelay = systeminfo['udelay']
#load system
x0 = np.array([[math.pi], [0.0]])
sys = Pendulum(systeminfo['params'])
#make input
print 'making data...'
M = 3
n = 50
uscale = 5.0
u = []
for i in range(0, M):
# u=np.hstack([u,math.pow(-1.0,i)*np.sin(2.0*dt*np.linspace(1.0,n,num=n))*uscale])
u = np.hstack([
u, -math.pow(-1.0, i) *
np.sin(0.01 * dt * np.power(np.linspace(1.0, n, num=n), 2)) * uscale
])
u = np.hstack([
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()):
