from math import sin, cos, pi from numpy import array, arange from visual import graph, sphere, curve, rate, display, color #import OSC #client = OSC.OSCClient() #client.connect( ('127.0.0.1', port_number) ) def f(X,t): theta1 = X[0] omega1 = X[1] theta2 = X[2] omega2 = X[3] f_theta1 = omega1 f_omega1 = -(omega1**2*sin(2*theta1-2*theta2)+2*omega2**2*sin(theta1-theta2)+\ (g/L)*sin(theta1-2*theta2)+(g/L)*3*sin(theta1))/\ (3-cos(2*theta1-2*theta2)) f_theta2 = omega2 f_omega2 = (4*omega1**2*sin(theta1-theta2)+omega2**2*sin(2*theta1-2*theta2)+\ 2*(g/L)*sin(2*theta1-theta2)-2*(g/L)*sin(theta2))/\ (3-cos(2*theta1-2*theta2)) return array( [f_theta1, f_omega1, f_theta2, f_omega2] ) def NRG(X): T = m*L**2*(X[1]**2+0.5*X[3]**2+X[1]*X[3]*cos(X[0]-X[2])) U = -m*g*L*(2*cos(X[0])+cos(X[2])) return T+U ########################### change this stuff for fun ################################# g = 9.81 # gravity L = 0.3 # pendulum arm in meters omega1 = 0.0 # initial speed of pendulum omega2 = 0.0 theta1 = pi # initial position of pendulum theta2 = pi/4 m = 1.0 # bob mass (kg) X = array( [theta1, omega1, theta2, omega2] ) a = 0.0 # starting time b = 100.0 # ending time n = 100000 # number of steps h = (b-a)/n # step size d = display(x = 500, y = 400) # moves window on computer monitor center = sphere(display = d, pos = (0,0,0), radius = 0.01, color = color.red) platform = curve(display = d, pos = [(-2*L,0,0), (2*L,0,0)], radius = 0.003) bob1 = sphere(display = d, pos = (L*cos(theta1-pi/2),L*sin(theta1-pi/2),0), radius = 0.03, color = color.blue) arm1 = curve(display = d, pos = [(0,0,0), (L*cos(theta1-pi/2),L*sin(theta1-pi/2),0)], radius = 0.005, color = color.yellow) bob2 = sphere(display = d, pos = (L*cos(theta1-pi/2)+L*cos(theta2-pi/2), L*sin(theta1-pi/2)+L*sin(theta2-pi/2),0), radius = 0.03, color = color.blue) arm2 = curve(display = d, pos = [(L*cos(theta1-pi/2),L*sin(theta1-pi/2),0), (L*cos(theta1-pi/2)+L*cos(theta2-pi/2), L*sin(theta1-pi/2)+L*sin(theta2-pi/2),0)], radius = 0.005, color = color.yellow) d = graph.gdisplay() d1 = graph.gdisplay() c = graph.gcurve(gdisplay = d) c1 = graph.gcurve(gdisplay = d1) c2 = graph.gcurve(gdisplay = d1) for t in arange(a,b,h): #if (t/h)%20 == 0: rate(1000) #c.plot(pos = [t,NRG(X)]) c1.plot(pos = [t,X[0]]) c2.plot(pos = [t,X[2]]) bob1.pos = [L*cos(X[0]-pi/2),L*sin(X[0]-pi/2),0] arm1.pos = [(0,0,0), (L*cos(X[0]-pi/2),L*sin(X[0]-pi/2),0)] bob2.pos = [L*cos(X[0]-pi/2)+L*cos(X[2]-pi/2),L*sin(X[0]-pi/2)+L*sin(X[2]-pi/2),0] arm2.pos = [(L*cos(X[0]-pi/2),L*sin(X[0]-pi/2),0), (L*cos(X[0]-pi/2)+L*cos(X[2]-pi/2),L*sin(X[0]-pi/2)+L*sin(X[2]-pi/2),0)] ########### Execute this code whenever you want to send a message ############### #msg = OSC.OSCMessage() # we reuse the same variable msg used above overwriting it #msg.setAddress("/test") #msg.append(123) #client.send(msg) # now we dont need to tell the client the address anymore k1 = h*f(X,t) k2 = h*f(X+k1/2, t+h/2) k3 = h*f(X+k2/2, t+h/2) k4 = h*f(X+k3, t+h) X += 1.0*(k1 + 2*k2 + 2*k3 + k4)/6