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()):