<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 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
Step1:  Mandamos llamar al simulador
<ASSISTANT_TASK:>
Python Code:
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]
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")
Explanation: 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)
$$
End of explanation
from robots.simuladores import simulador
%matplotlib widget
ts, xs = simulador(puerto_zmq="5551", f=f, x0=[0, 0, 0, 0], dt=0.02)
Explanation: Mandamos llamar al simulador
End of explanation
<END_TASK>