from generacion_trayectorias import grafica_trayectoria
%matplotlib inline

"""
Explanation: Generación de trayectorias por medio de LSPB
El objetivo de esta práctica es generar una trayectoria para un robot manipulador, de tal manera que no tenga cambios bruscos de posición o velocidad.
El algoritmo general que vamos a usar se llama LSPB (Linear segment with parabolic blend), en el cual vamos a tener una velocidad de crucero para el manipulador, así como un periodo en el que el manipulador acelerará constantemente y otro periodo en el que desacelererá.
Una trayectoria generada por este método se ve asi:

En la primer sección se tiene una aceleración constante, en la segunda una velocidad constante y en la tercera una aceleración constante de signo contrario a la primera.
El método por el que se generó no es particularmente dificil, tan solo es un poco engorroso de programar, por lo que para facilidad de esta práctica, este método ya esta programado, tan solo hay que importar el código:
End of explanation
"""


ts, qs, q̇s, q̈s = grafica_trayectoria(0, 2, 0, 1, 1000)

"""
Explanation: Vamos a hacer una prueba en primer lugar:
End of explanation
"""


from numpy import pi
τ = 2*pi

"""
Explanation: En la gráfica anterior podemos ver no solo la posición en el primer cuadro, si no tambien la velocidad y la aceleración en el segundo y tercero, de tal manera que nos damos una mejor idea de la trayectoria.
Vamos a generar un conjunto de trayectorias para un ejemplo, primero empecemos importando pi:
End of explanation
"""


ts, q1, q̇1, q̈1 = grafica_trayectoria(0, 2, 0, τ/4, 100)

ts, q2, q̇2, q̈2 = grafica_trayectoria(2, 4, τ/4, -τ/6, 100)

ts, q3, q̇3, q̈3 = grafica_trayectoria(4, 10, -τ/6, 2*τ/3, 300)

"""
Explanation: Vamos a generar una trayectoria en la que en los primero dos segundos se mueva de $0^o$ a $90^o$, en los segundos dos segundos de $90^o$ a $-60^o$ y en los ultimos seis segundos de $-60^o$ a $240^o$:
End of explanation
"""


qs = q1 + q2 + q3

"""
Explanation: Si quiero concatenar todos estos arreglos que generé, tan solo tengo que sumarlos:
End of explanation
"""


from matplotlib.pyplot import figure, style

from numpy import linspace
fig = figure(figsize=(17, 5))
ax = fig.gca()
ts = linspace(0, 10, 500)
ax.plot(ts, qs)

"""
Explanation: Esta trayectoria la podemos graficar:
End of explanation
"""


# Se importa la funcion animation para crear la animacion, y rc para poder mostrar el video
# directamente en el notebook
from matplotlib import animation, rc
rc('animation', html='html5')
# Se importan las funciones necesarias para calcular la cinematica directa e inversa
from numpy import sin, cos, arange

# Se define una funcion para calcular la cinematica directa del sistema
def cinematica_directa_pendulo(q1):
    # Se definen constantes utilizadas para graficar el sistema
    l1, l2 = 1, 1
    xs = [0, l1*cos(q1)]
    ys = [0, l1*sin(q1)]
    return xs, ys

# Se define el tamaño de la figura
fig = figure(figsize=(8, 8))

# 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))

# Se utilizan graficas de linea para el eslabon del pendulo
linea, = axi.plot([], [], "-o", lw=2, color='gray')

def inicializacion():
    '''Esta funcion se ejecuta una sola vez y sirve para inicializar el sistema'''
    
    # Se inicializa la linea vacia para evitar que al principio exista una linea en la grafica
    linea.set_data([], [])
    return linea

def animacion(i):
    '''Esta funcion se ejecuta para cada cuadro del GIF'''
    
    # Se obtienen las coordenadas x y y para el eslabon
    xs, ys = cinematica_directa_pendulo(qs[i])
    # Se actualiza el estado de la linea con las coordenadas calculadas
    linea.set_data(xs, ys)
    
    return linea

# Se hace la animacion dandole 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, animacion, arange(1, len(qs)), interval=20, init_func=inicializacion)
ani

"""
Explanation: Pero mas importante, puedo generar una animación, tomando en cuenta que es un pendulo simple:
End of explanation
"""


