|
| 1 | +# Double pendulum formula translated from the C code at |
| 2 | +# http://www.physics.usyd.edu.au/~wheat/dpend_html/solve_dpend.c |
| 3 | + |
| 4 | +from numpy import sin, cos, pi, array |
| 5 | +import numpy as np |
| 6 | +import matplotlib.pyplot as plt |
| 7 | +import scipy.integrate as integrate |
| 8 | +import matplotlib.animation as animation |
| 9 | + |
| 10 | +G = 9.8 # acceleration due to gravity, in m/s^2 |
| 11 | +L1 = 1.0 # length of pendulum 1 in m |
| 12 | +L2 = 1.0 # length of pendulum 2 in m |
| 13 | +M1 = 1.0 # mass of pendulum 1 in kg |
| 14 | +M2 = 1.0 # mass of pendulum 2 in kg |
| 15 | + |
| 16 | + |
| 17 | +def derivs(state, t): |
| 18 | + |
| 19 | + dydx = np.zeros_like(state) |
| 20 | + dydx[0] = state[1] |
| 21 | + |
| 22 | + del_ = state[2]-state[0] |
| 23 | + den1 = (M1+M2)*L1 - M2*L1*cos(del_)*cos(del_) |
| 24 | + dydx[1] = (M2*L1*state[1]*state[1]*sin(del_)*cos(del_) |
| 25 | + + M2*G*sin(state[2])*cos(del_) + M2*L2*state[3]*state[3]*sin(del_) |
| 26 | + - (M1+M2)*G*sin(state[0]))/den1 |
| 27 | + |
| 28 | + dydx[2] = state[3] |
| 29 | + |
| 30 | + den2 = (L2/L1)*den1 |
| 31 | + dydx[3] = (-M2*L2*state[3]*state[3]*sin(del_)*cos(del_) |
| 32 | + + (M1+M2)*G*sin(state[0])*cos(del_) |
| 33 | + - (M1+M2)*L1*state[1]*state[1]*sin(del_) |
| 34 | + - (M1+M2)*G*sin(state[2]))/den2 |
| 35 | + |
| 36 | + return dydx |
| 37 | + |
| 38 | +# create a time array from 0..100 sampled at 0.1 second steps |
| 39 | +dt = 0.05 |
| 40 | +t = np.arange(0.0, 20, dt) |
| 41 | + |
| 42 | +# th1 and th2 are the initial angles (degrees) |
| 43 | +# w10 and w20 are the initial angular velocities (degrees per second) |
| 44 | +th1 = 120.0 |
| 45 | +w1 = 0.0 |
| 46 | +th2 = -10.0 |
| 47 | +w2 = 0.0 |
| 48 | + |
| 49 | +rad = pi/180 |
| 50 | + |
| 51 | +# initial state |
| 52 | +state = np.array([th1, w1, th2, w2])*pi/180. |
| 53 | + |
| 54 | +# integrate your ODE using scipy.integrate. |
| 55 | +y = integrate.odeint(derivs, state, t) |
| 56 | + |
| 57 | +x1 = L1*sin(y[:,0]) |
| 58 | +y1 = -L1*cos(y[:,0]) |
| 59 | + |
| 60 | +x2 = L2*sin(y[:,2]) + x1 |
| 61 | +y2 = -L2*cos(y[:,2]) + y1 |
| 62 | + |
| 63 | +fig = plt.figure() |
| 64 | +ax = fig.add_subplot(111, autoscale_on=False, xlim=(-2, 2), ylim=(-2, 2)) |
| 65 | +ax.grid() |
| 66 | + |
| 67 | +line, = ax.plot([], [], 'o-', lw=2) |
| 68 | +time_template = 'time = %.1fs' |
| 69 | +time_text = ax.text(0.05, 0.9, '', transform=ax.transAxes) |
| 70 | + |
| 71 | +def init(): |
| 72 | + line.set_data([], []) |
| 73 | + time_text.set_text('') |
| 74 | + return line, time_text |
| 75 | + |
| 76 | +def animate(i): |
| 77 | + thisx = [0, x1[i], x2[i]] |
| 78 | + thisy = [0, y1[i], y2[i]] |
| 79 | + |
| 80 | + line.set_data(thisx, thisy) |
| 81 | + time_text.set_text(time_template%(i*dt)) |
| 82 | + return line, time_text |
| 83 | + |
| 84 | +ani = animation.FuncAnimation(fig, animate, np.arange(1, len(y)), |
| 85 | + interval=25, blit=True, init_func=init) |
| 86 | + |
| 87 | +plt.show() |
0 commit comments