#39 Double pendulum using python

import os
import numpy as np
import matplotlib.pyplot as plt
from scipy import integrate
import sympy
os.chdir(r’D:\projects\wordpress\ex39′)
os.getcwd()
t, g, m1, l1, m2, l2 = sympy.symbols(“t, g, m_1, l_1, m_2, l_2”)
theta1, theta2 = sympy.symbols(“theta_1, theta_2”, cls=sympy.Function)
ode1 = sympy.Eq((m1+m2)*l1 * theta1(t).diff(t,t) +
m2*l2 * theta2(t).diff(t,t) +
m2*l2 * theta2(t).diff(t)**2 * sympy.sin(theta1(t)-theta2(t)) +
g*(m1+m2) * sympy.sin(theta1(t)))
ode1
ode2 = sympy.Eq(m2*l2 * theta2(t).diff(t,t) +
m2*l1 * theta1(t).diff(t,t) * sympy.cos(theta1(t)-theta2(t)) –
m2*l1 * theta1(t).diff(t)**2 * sympy.sin(theta1(t) – theta2(t)) +
m2*g * sympy.sin(theta2(t)))
ode2
# sympy cannot solve these ODEs
try:
sympy.dsolve(ode1, ode2)
except Exception as e:
print(e)

y1, y2, y3, y4 = sympy.symbols(“y_1, y_2, y_3, y_4”, cls=sympy.Function)
varchange = {theta1(t).diff(t, t): y2(t).diff(t),
theta1(t): y1(t),
theta2(t).diff(t, t): y4(t).diff(t),
theta2(t): y3(t)}
ode1_vc = ode1.subs(varchange)
ode2_vc = ode2.subs(varchange)
ode3 = y1(t).diff(t) – y2(t)
ode4 = y3(t).diff(t) – y4(t)
y = sympy.Matrix([y1(t), y2(t), y3(t), y4(t)])
vcsol = sympy.solve((ode1_vc, ode2_vc, ode3, ode4), y.diff(t), dict=True)
f = y.diff(t).subs(vcsol[0])
sympy.Eq(y.diff(t), f)
params = {m1: 5.0, l1: 2.0,
m2: 1.0, l2: 1.0, g: 10.0}
f_np = sympy.lambdify((t, y), f.subs(params), ‘numpy’)
jac = sympy.Matrix([[fj.diff(yi) for yi in y] for fj in f])
jac_np = sympy.lambdify((t, y), jac.subs(params), ‘numpy’)
y0 = [2.0, 0, 0.0, 0] t = np.linspace(0, 20, 1000)
jac_np(0, y0)
r = integrate.ode(f_np, jac_np).set_initial_value(y0, t[0]);
dt = t[1] – t[0] y = np.zeros((len(t), len(y0)))
idx = 0
while r.successful() and r.t < t[-1]:
y[idx, :] = r.y
r.integrate(r.t + dt)
idx += 1

fig = plt.figure(figsize=(10, 4))
ax1 = plt.subplot2grid((2, 5), (0, 0), colspan=3)
ax2 = plt.subplot2grid((2, 5), (1, 0), colspan=3)
ax3 = plt.subplot2grid((2, 5), (0, 3), colspan=2, rowspan=2)
ax1.plot(t, y[:, 0], ‘r’)
ax1.set_ylabel(r’$\theta_1$’, fontsize=18)
ax2.plot(t, y[:, 2], ‘b’)
ax2.set_xlabel(‘$t$’, fontsize=18)
ax2.set_ylabel(r’$\theta_2$’, fontsize=18)
ax3.plot(y[:, 0], y[:, 2], ‘k’)
ax3.set_xlabel(r’$\theta_1$’, fontsize=18)
ax3.set_ylabel(r’$\theta_2$’, fontsize=18)
fig.tight_layout()
plt.savefig(“example39.png”, dpi=100)

theta1_np, theta2_np = y[:, 0], y[:, 2] x1 = params[l1] * np.sin(theta1_np)
y1 = -params[l1] * np.cos(theta1_np)
x2 = x1 + params[l2] * np.sin(theta2_np)
y2 = y1 – params[l2] * np.cos(theta2_np)
fig = plt.figure(figsize=(10, 4))
ax1 = plt.subplot2grid((2, 5), (0, 0), colspan=3)
ax2 = plt.subplot2grid((2, 5), (1, 0), colspan=3)
ax3 = plt.subplot2grid((2, 5), (0, 3), colspan=2, rowspan=2)
ax1.plot(t, x1, ‘r’)
ax1.plot(t, y1, ‘b’)
ax1.set_ylabel(‘$x_1, y_1$’, fontsize=18)
ax1.set_yticks([-3, 0, 3])
ax2.plot(t, x2, ‘r’)
ax2.plot(t, y2, ‘b’)
ax2.set_xlabel(‘$t$’, fontsize=18)
ax2.set_ylabel(‘$x_2, y_2$’, fontsize=18)
ax2.set_yticks([-3, 0, 3])
ax3.plot(x1, y1, ‘r’)
ax3.plot(x2, y2, ‘b’, lw=0.5)
ax3.set_xlabel(‘$x$’, fontsize=18)
ax3.set_ylabel(‘$y$’, fontsize=18)
ax3.set_xticks([-3, 0, 3])
ax3.set_yticks([-3, 0, 3])
fig.tight_layout()
plt.savefig(“example39_2.png”, dpi=100)
plt.show()
plt.close()