-
Notifications
You must be signed in to change notification settings - Fork 54
/
double_pendulum.py
76 lines (64 loc) · 1.99 KB
/
double_pendulum.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
#!/usr/bin/env python
# -*- animation -*-
"""
Animation of a double pendulum
"""
from numpy import sin, cos, pi, array
import time
import gr
g = 9.8 # gravitational constant
def rk4(x, h, y, f):
k1 = h * f(x, y)
k2 = h * f(x + 0.5 * h, y + 0.5 * k1)
k3 = h * f(x + 0.5 * h, y + 0.5 * k2)
k4 = h * f(x + h, y + k3)
return x + h, y + (k1 + 2 * (k2 + k3) + k4) / 6.0
def pendulum_derivs(t, state):
# The following derivation is from:
# http://scienceworld.wolfram.com/physics/DoublePendulum.html
t1, w1, t2, w2 = state
a = (m1 + m2) * l1
b = m2 * l2 * cos(t1 - t2)
c = m2 * l1 * cos(t1 - t2)
d = m2 * l2
e = -m2 * l2 * w2**2 * sin(t1 - t2) - g * (m1 + m2) * sin(t1)
f = m2 * l1 * w1**2 * sin(t1 - t2) - m2 * g * sin(t2)
return array([w1, (e*d-b*f) / (a*d-c*b), w2, (a*f-c*e) / (a*d-c*b)])
def pendulum(theta, length, mass):
l = length[0] + length[1]
gr.clearws()
gr.setviewport(0, 1, 0, 1)
gr.setwindow(-l, l, -l, l)
gr.setmarkertype(gr.MARKERTYPE_SOLID_CIRCLE)
gr.setmarkercolorind(86)
pivot = [0, 0.775] # draw pivot point
gr.fillarea(4, [-0.2, 0.2, 0.2, -0.2], [0.75, 0.75, 0.8, 0.8])
for i in range(2):
x = [pivot[0], pivot[0] + sin(theta[i]) * length[i]]
y = [pivot[1], pivot[1] - cos(theta[i]) * length[i]]
gr.polyline(2, x, y) # draw rod
gr.setmarkersize(3 * mass[i])
gr.polymarker(1, [x[1]], [y[1]]) # draw bob
pivot = [x[1], y[1]]
gr.updatews()
return
l1 = 1.2 # length of rods
l2 = 1.0
m1 = 1.0 # weights of bobs
m2 = 1.5
t1 = 100.0 # inintial angles
t2 = -20.0
w1 = 0.0
w2 = 0.0
t = 0
dt = 0.04
state = array([t1, w1, t2, w2]) * pi / 180
now = time.clock()
while t < 30:
start = now
t, state = rk4(t, dt, state, pendulum_derivs)
t1, w1, t2, w2 = state
pendulum([t1, t2], [l1, l2], [m1, m2])
now = time.clock()
if start + dt > now:
time.sleep(start + dt - now)