<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 Problemas
Defina una función que obtenga la cinemática inversa de un pendulo doble.
Step1:  Obtenga las posiciones en el espacio articular, $q_1$ y $q_2$, necesarias para que el punto final del pendulo doble llegue a las coordenadas $p_1 = (0,1)$, $p_2 = (1,3)$ y $p_3 = (3,2)$.
Step2:  Genere las trayectorias necesarias para que el pendulo doble se mueva del punto $p_1$ al punto $p_2$ en $2s$, del punto $p_2$ al punto $p_3$ en $2s$ y del punto $p_3$ al punto $p_1$ en $2s$.
Utiliza 100 puntos por segundo y asegurate de guardar las trayectorias generadas en las variables correctas para que q1s y q2s tengan las trayectorias completas.
Step3:  Cree una animación con las trayectorias generadas y las funciones proporcionadas a continuación (algunas funciones estan marcadas con comentarios en donde hace falta agregar código).
<ASSISTANT_TASK:>
Python Code:
def ci_pendulo_doble(x, y):
    # tome en cuenta que las longitudes de los eslabones son 2 y 2
    l1, l2 = 2, 2
    from numpy import arccos, arctan2, sqrt
    # YOUR CODE HERE
    raise NotImplementedError()
    return q1, q2
from numpy.testing import assert_allclose
assert_allclose(ci_pendulo_doble(4, 0), (0,0))
assert_allclose(ci_pendulo_doble(0, 4), (1.57079632,0))
Explanation: Problemas
Defina una función que obtenga la cinemática inversa de un pendulo doble.
End of explanation
# YOUR CODE HERE
raise NotImplementedError()
from numpy.testing import assert_allclose
assert_allclose((q11, q21),(0.25268 , 2.636232), rtol=1e-05, atol=1e-05)
from numpy.testing import assert_allclose
assert_allclose((q12, q22),(0.589988, 1.318116), rtol=1e-05, atol=1e-05)
from numpy.testing import assert_allclose
assert_allclose((q13, q23),(0.14017 , 0.895665), rtol=1e-05, atol=1e-05)
Explanation: Obtenga las posiciones en el espacio articular, $q_1$ y $q_2$, necesarias para que el punto final del pendulo doble llegue a las coordenadas $p_1 = (0,1)$, $p_2 = (1,3)$ y $p_3 = (3,2)$.
End of explanation
from generacion_trayectorias import grafica_trayectoria
# YOUR CODE HERE
raise NotImplementedError()
q1s = q1s1 + q1s2 + q1s3
q2s = q2s1 + q2s2 + q2s3
from numpy.testing import assert_allclose
assert_allclose((q1s[0], q1s[-1]),(0.25268, 0.25268), rtol=1e-05, atol=1e-05)
from numpy.testing import assert_allclose
assert_allclose((q2s[0], q2s[-1]),(2.636232, 2.636232), rtol=1e-05, atol=1e-05)
Explanation: Genere las trayectorias necesarias para que el pendulo doble se mueva del punto $p_1$ al punto $p_2$ en $2s$, del punto $p_2$ al punto $p_3$ en $2s$ y del punto $p_3$ al punto $p_1$ en $2s$.
Utiliza 100 puntos por segundo y asegurate de guardar las trayectorias generadas en las variables correctas para que q1s y q2s tengan las trayectorias completas.
End of explanation
from matplotlib.pyplot import figure, style
from matplotlib import animation, rc
rc('animation', html='html5')
from numpy import sin, cos, arange
fig = figure(figsize=(8, 8))
axi = fig.add_subplot(111, autoscale_on=False, xlim=(-0.6, 3.1), ylim=(-0.6, 3.1))
linea, = axi.plot([], [], "-o", lw=2, color='gray')
def cd_pendulo_doble(q1, q2):
    l1, l2 = 2, 2
    # YOUR CODE HERE
    raise NotImplementedError()
    return xs, ys
def inicializacion():
    '''Esta funcion se ejecuta una sola vez y sirve para inicializar el sistema'''
    linea.set_data([], [])
    return linea
def animacion(i):
    '''Esta funcion se ejecuta para cada cuadro del GIF'''
    # YOUR CODE HERE
    raise NotImplementedError()
    linea.set_data(xs, ys)
    
    return linea
ani = animation.FuncAnimation(fig, animacion, arange(1, len(q1s)), interval=10, init_func=inicializacion)
ani
from numpy.testing import assert_allclose
assert_allclose(cd_pendulo_doble(0, 0), ([0,2,4], [0,0,0]), rtol=1e-05, atol=1e-05)
assert_allclose(cd_pendulo_doble(1.57079632,0), ([0, 0, 0],[0, 2, 4]), rtol=1e-05, atol=1e-05)
Explanation: Cree una animación con las trayectorias generadas y las funciones proporcionadas a continuación (algunas funciones estan marcadas con comentarios en donde hace falta agregar código).
End of explanation
<END_TASK>