Recordando los resultados obtenidos en el documento anterior, tenemos que:
$$ \tau_1 = \left[ m_1 l_1^2 + J_1 \right] \ddot{q}_1 + g\left[ m_1 l_1 \cos(q_1) \right] $$sin embargo, no tenemos una buena manera de simular el comportamiento de este sistema ya que este sistema es de segundo orden y no lineal; por lo que el primer paso para simular este sistema es convertirlo a un sistema de primer orden.
Primero necesitamos definir una variable de estado para el sistema, y ya que queremos convertir un sistema de segundo orden, en uno de primer orden, necesitamos duplicar el numero de variables.
Nota: Si tuvieramos un sistema de tercer orden tendriamos que triplicar el numero de variables para convertirlo en un sistema de primer orden y asi sucesivamente.
Si definimos el estado del sistema $x$ como:
$$ x = \begin{pmatrix} q_1 \\ \dot{q}_1 \end{pmatrix} $$entonces es claro que podemos obtener la derivada del sistema haciendo:
$$ \dot{x} = \frac{d}{dt} x = \frac{d}{dt} \begin{pmatrix} q_1 \\ \dot{q}_1 \end{pmatrix} = \begin{pmatrix} \dot{q}_1 \\ \ddot{q}_1 \end{pmatrix} $$por lo que podemos utilizar métodos numéricos como el método de Euler, Runge-Kutta, etc. para obtener el comportamiento de estas variables a traves del tiempo, tomando en cuenta que siempre es necesario tener una función $f$ tal que:
$$ \dot{x} = f(x,t) $$Nuestra tarea entonces, es construir una función f, tal que, cuando le demos como datos $x = \begin{pmatrix} q_1 \\ \dot{q}_1 \end{pmatrix}$, nos pueda devolver como resultado $\dot{x} = \begin{pmatrix} \dot{q}_1 \\ \ddot{q}_1 \end{pmatrix}$.
Para esto necesitamos calcular $\dot{q}_1$ y $\ddot{q}_1$, pero si revisamos la ecuación de movimiento calculada en el documento anterior (simbolico.ipynb), podemos ver que $\ddot{q}_1$ es facilmente despejable:
Para calcular $\dot{q}_1$, tan solo tenemos que darnos cuenta que uno de nuestros datos es $\dot{q}_1$, por lo que esta ecuación es trivial:
$$ \dot{q}_1 = \dot{q}_1 $$con lo que ya tenemos todo lo necesario para construir la función f:
In [ ]:
def f(x, t):
# Se importan funciones matematicas necesarias
from numpy import cos
# Se desenvuelven las variables que componen al estado
q1, q̇1 = x
# Se definen constantes del sistema
g = 9.81
m1, J1 = 0.3, 0.0005
l1 = 0.2
τ1 = 0
# Se calculan las variables a devolver por el sistema
q1p = q̇1
q1pp = 1/(m1*l1**2 + J1)*(τ1 - g*m1*l1*cos(q1))
# Se devuelve la derivada de las variables de entrada
return [q1p, q1pp]
Una vez que tenemos la función f, tenemos que importar la función odeint de la librería integrate de scipy.
In [ ]:
from scipy.integrate import odeint
Esta función implementará un método numérico para integrar el estado del sistema a traves del tiempo, definiremos tambien un vector con todos los tiempos en los que nos interesa saber el estado del sistema, para esto importamos linspace.
In [ ]:
from numpy import linspace
Definimos los tiempos como $0s$ hasta $10s$ con mil datos en total, y la condición inicial de nuestro sistema:
$$ x_0 = \begin{pmatrix} 0 \\ 0 \end{pmatrix} = \begin{pmatrix} q_1 \\ \dot{q}_1 \end{pmatrix} $$
In [ ]:
ts = linspace(0, 10, 1000)
x0 = [0, 0]
Y con estos datos, mandamos llamar a la función odeint:
In [ ]:
xs = odeint(f, x0, ts)
Para visualizar estos datos importamos la libreria matplotlib para graficación:
In [ ]:
from matplotlib.pyplot import plot, legend
%matplotlib widget
Y graficamos:
In [ ]:
graf_pos, graf_vel = plot(xs)
legend([graf_pos, graf_vel], ["posicion", "velocidad"])
Sin embargo, en esta clase utilizaremos un sistema de simulación a medida, el cual nos quita la responsabilidad del codigo de graficación y nos brindará algunas ventajas en practicas posteriores. A continuación importamos la función simulador de la librería simuladores del paquete robots:
In [ ]:
from robots.simuladores import simulador
Nota que dentro de la carpeta de esta práctica existe una carpeta llamada robots, esta carpeta contiene todo el código que hace funcionar las siguientes funciones, si deseas averiguar como funciona, puedes abrir esos archivos y estudiarlos.
Definimos de nuevo la función f con la única diferencia del orden de las entradas:
In [ ]:
def f(t, x):
# Se importan funciones matematicas necesarias
from numpy import cos
# Se desenvuelven las variables que componen al estado
q1, q̇1 = x
# Se definen constantes del sistema
g = 9.81
m1, J1 = 0.3, 0.0005
l1 = 0.4
τ1 = 0
# Se calculan las variables a devolver por el sistema
q1p = q̇1
q1pp = 1/(m1*l1**2 + J1)*(τ1 - g*m1*l1*cos(q1))
# Se devuelve la derivada de las variables de entrada
return [q1p, q1pp]
Mandamos llamar al simulador
In [ ]:
%matplotlib widget
ts, xs = simulador(puerto_zmq="5551", f=f, x0=[0,0], dt=0.02)
El argumento
puerto_zmqse refiere al puerto por el cual esta mandando datos, para el visualizador descrito a continuación.
Estos datos se estan actualizando en tiempo real, y si bien es posible ver el comportamiento general con esta gráfica, tambien es posible ver una visualización en tiempo real de este manipulador, para lo cual es necesario mantener este documento abierto, mientras se abre el documento visualizacion.ipynb.