Práctica 2 - Cinemática directa y dinámica de manipuladores

Una vez obtenida la dinámica del manipulador, tenemos la necesidad de construir una función f para poder simular el comportamiento del manipulador, empecemos escribiendo la ecuación:

$$ \tau = \begin{bmatrix} J_1 + J_2 + m_1 l_1^2 + m_2 l_1^2 + \mu_1 + 2 \mu_2 c_2 & J_2 + \mu_1 + \mu_2 c_2 \\ J_2 + \mu_1 + \mu_2 c_2 & J_2 + \mu_1 \end{bmatrix}\ddot{q} - \mu_2 s_2 \begin{bmatrix} 2 \dot{q}_2 & \dot{q}_2 \\ -\dot{q}_1 & 0 \end{bmatrix} + g \begin{bmatrix} m_1 l_1 c_1 + m_2 l_1 c_1 + m_2 l_2 c_{12} \\ m_2 l_2 c_{12} \end{bmatrix} $$

en donde $\mu_1 = m_2 l_2^2$ y $\mu_2 = m_2 l_1 l_2$; por lo que de aqui en adelante, podemos caracterizar la dinámica de este manipulador como la siguiente ecuación:

$$ \tau = M(q)\ddot{q} + C(q, \dot{q}) + G(q) $$

Si ahora cambiamos nuestra atención al problema de contruir la función

$$ \dot{x} = f(x, t) $$

tenemos que empezar por que representa el estado $x$.

En el ejercicio pasado nuestro manipulador tenía un solo grado de libertad, por lo que el estado terminaba siendo:

$$ x = \begin{pmatrix} q_1 \\ \dot{q}_1 \end{pmatrix} $$

En este caso, nuestro manipulador tiene dos grados de libertad, por lo que necesitamos que el estado incluya a la posición de ambos grados de libertad, así como su velocidad:

$$ x = \begin{pmatrix} q_1 \\ q_2 \\ \dot{q}_1 \\ \dot{q}_2 \end{pmatrix} $$

Por lo que para construir $f(x,t)$, necesitamos calcular los siguientes terminos:

$$ \dot{x} = \begin{pmatrix} \dot{q}_1 \\ \dot{q}_2 \\ \ddot{q}_1 \\ \ddot{q}_2 \end{pmatrix} $$

en donde los primeros dos terminos son triviales, ya que son los mismos que obtenemos del estado del sistema ($\dot{q}_1$, $\dot{q}_2$), y los segundos dos terminos los podemos obtener de la ecuación de movimiento del manipulador:

$$ \ddot{q} = M^{-1}\left( \tau - C(q, \dot{q})\dot{q} - G(q) \right) $$



In [ ]:

    
def f(t, x):
    # Se importan funciones matematicas necesarias
    from numpy import matrix, sin, cos
    # Se desenvuelven las variables que componen al estado
    q1, q2, q̇1, q̇2 = x
    # Se definen constantes del sistema
    g = 9.81
    m1, m2, J1, J2 = 0.3, 0.2, 0.0005, 0.0002
    l1, l2 = 0.4, 0.3
    τ1, τ2 = 0, 0
    # Se agrupan terminos en vectores
    q̇ = matrix([[q̇1], [q̇2]])
    τ = matrix([[τ1], [τ2]])
    # Se calculan terminos comúnes
    μ1 = m2*l2**2
    μ2 = m2*l1*l2
    c1 = cos(q1)
    c2 = cos(q2)
    s2 = sin(q2)
    c12 = cos(q1 + q2)
    # Se calculan las matrices de la ecuación de movimiento
    # ESCRIBE TU CODIGO AQUI
    raise NotImplementedError
    # Se calculan las variables a devolver por el sistema
    # ESCRIBE TU CODIGO AQUI
    raise NotImplementedError
    q1pp = qpp.item(0)
    q2pp = qpp.item(1)
    # Se devuelve la derivada de las variables de entrada
    return [q1p, q2p, q1pp, q2pp]



In [ ]:

    
from numpy.testing import assert_almost_equal
assert_almost_equal(f(0, [0, 0, 0, 0]), [0,0,-1392.38, 3196.16], 2)
assert_almost_equal(f(0, [1, 1, 0, 0]), [0,0,-53.07, 104.34], 2)
print("Sin errores")

Mandamos llamar al simulador



In [ ]:

    
from robots.simuladores import simulador



In [ ]:

    
%matplotlib widget
ts, xs = simulador(puerto_zmq="5551", f=f, x0=[0, 0, 0, 0], dt=0.02)

El argumento puerto_zmq se 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.