In [1]:
from sympy import var, sin, cos, Matrix, Integer, eye, Function, Rational, exp, Symbol, I, solve, pi, trigsimp, dsolve, sinh, cosh, simplify
from sympy.physics.mechanics import mechanics_printing
mechanics_printing()
In [2]:
var("m1 m2 J1 J2 l1 l2 L1 L2 t g")
Out[2]:
In [3]:
q1 = Function("q1")(t)
q2 = Function("q2")(t)
In [4]:
x1 = l1*cos(q1)
y1 = l1*sin(q1)
v1 = x1.diff("t")**2 + y1.diff("t")**2
v1.trigsimp()
Out[4]:
In [5]:
x2 = L1*cos(q1) + l2*cos(q1 + q2)
y2 = L1*sin(q1) + l2*sin(q1 + q2)
v2 = x2.diff("t")**2 + y2.diff("t")**2
v2.trigsimp()
Out[5]:
In [6]:
ω1, ω2 = q1.diff("t"), q1.diff("t") + q2.diff("t")
In [7]:
K1 = Rational(1, 2)*m1*v1 + Rational(1, 2)*J1*ω1**2
K1
Out[7]:
In [8]:
K2 = Rational(1, 2)*m2*v2 + Rational(1, 2)*J2*ω2**2
K2
Out[8]:
In [9]:
U1 = m1*g*y1
U1
Out[9]:
In [10]:
U2 = m2*g*y2
U2
Out[10]:
In [11]:
K = K1 + K2
K
Out[11]:
In [12]:
U = U1 + U2
U
Out[12]:
In [13]:
L = (K - U).expand().simplify()
L
Out[13]:
In [14]:
τ1 = (L.diff(q1.diff(t)).diff(t) - L.diff(q1)).simplify().expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))
In [15]:
τ2 = (L.diff(q2.diff(t)).diff(t) - L.diff(q2)).simplify().expand().collect(q1.diff(t).diff(t)).collect(q2.diff(t).diff(t))
In [16]:
τ1
Out[16]:
In [17]:
τ2
Out[17]:
In [18]:
from scipy.integrate import odeint
from numpy import linspace
In [19]:
def pendulo_doble(estado, tiempo):
# Se importan funciones necesarias
from numpy import sin, cos, matrix
# Se desenvuelven variables del estado y tiempo
q1, q2, q̇1, q̇2 = estado
t = tiempo
# Se declaran constantes del sistema
m1, m2 = 0.3, 0.5
l1, l2 = 0.3, 0.2
L1, L2 = 0.5, 0.4
J1, J2 = 0.017, 0.005
g = 9.81
# Señales de control nulas
tau1, tau2 = 0, 0
# Se calculan algunos terminos comunes
μ1 = m2*l2**2
μ2 = m2*L1*l2
c1, c2, s2 = cos(q1), cos(q2), sin(q2)
c12 = cos(q1 + q2)
# Se calculan las matrices de masas, Coriolis,
# y vectores de gravedad, control, posicion y velocidad
M = matrix([[m1*l1**2 + m2*L1**2 + μ1 + 2*μ2*c2 + J1 + J2, μ1 + μ2*c2 + J2],
[μ1 + μ2*c2 + J2, μ1 + J2]])
C = -μ2*s2*matrix([[2*q̇2, q̇2],
[-q̇1, 0]])
G = matrix([[m1*l1*c1 + m2*L1*c1 + m2*l2*c12],
[m2*l2*c12]])
Tau = matrix([[tau1],
[tau2]])
q = matrix([[q1],
[q2]])
q̇ = matrix([[q̇1],
[q̇2]])
# Se calcula la derivada del estado del sistema
qp1 = q̇1
qp2 = q̇2
qpp = M.I*(Tau - C*q̇ - G)
qpp1, qpp2 = qpp.tolist()
return [qp1, qp2, qpp1[0], qpp2[0]]
In [20]:
tiempos = linspace(0, 10, 1000)
cond_iniciales = [0, 0, 0, 0]
estados_simulados = odeint(func=pendulo_doble, y0=cond_iniciales, t=tiempos)
In [21]:
q1, q2, q̇1, q̇2 = list(zip(*estados_simulados.tolist()))
In [22]:
%matplotlib inline
from matplotlib.pyplot import plot, style, figure
from mpl_toolkits.mplot3d import Axes3D
style.use("ggplot")
In [23]:
fig1 = figure(figsize=(16, 8))
ax1 = fig1.gca()
p1, = ax1.plot(tiempos, q1)
p2, = ax1.plot(tiempos, q2)
ax1.legend([p1, p2],[r"$q_1$", r"$q_2$"])
#ax1.set_ylim(-4.1, 6.1)
ax1.set_xlim(-0.1, 10.1);
In [24]:
fig1 = figure(figsize=(8, 8))
ax1 = fig1.gca()
p1, = ax1.plot(q1, q2)
ax1.set_ylim(-6.1, 6.1)
ax1.set_xlim(-6.1, 6.1);
In [25]:
from numpy import sin, cos, arange
from matplotlib import animation, rc
rc('animation', html='html5')
In [26]:
L1, L2 = 0.5, 0.4
# Se define el tamaño de la figura
fig = figure(figsize=(10, 10))
# Se define una sola grafica en la figura y se dan los limites de los ejes x y y
axi = fig.add_subplot(111, autoscale_on=False, xlim=(-1.1, 1.1), ylim=(-1.1, 1.1))
axi.set_xticklabels([])
axi.set_yticklabels([])
axi.axes.get_xaxis().set_visible(False)
axi.axes.get_yaxis().set_visible(False)
# Se utilizan graficas de linea para el eslabon del pendulo
linea, = axi.plot([], [], "-o", lw=2, color='gray')
def init():
# Esta funcion se ejecuta una sola vez y sirve para inicializar el sistema
linea.set_data([], [])
return linea
def animate(i):
# Esta funcion se ejecuta para cada cuadro del GIF
# Se obtienen las coordenadas x y y para el eslabon
xs, ys = [[0, L1*cos(q1[i]), L1*cos(q1[i]) + L2*cos(q1[i]+q2[i])],
[0, L1*sin(q1[i]), L1*sin(q1[i]) + L2*sin(q1[i]+q2[i])]]
linea.set_data(xs, ys)
return linea
# Se hace la animacion dandole el nombre de la figura definida al principio, la funcion que
# se debe ejecutar para cada cuadro, el numero de cuadros que se debe de hacer, el periodo
# de cada cuadro y la funcion inicial
ani = animation.FuncAnimation(fig, animate, arange(1, len(q1)), interval=10, init_func=init);
ani
Out[26]:
In [ ]: