Consideramos el problema de mover el link desde su posición inicial hasta una posición final deseada y que este movimiento va a ser realizado en un cierto tiempo($t$). Lo que vamos a plantear es una función cuyo valor en el tiempo inicial sea la posición inicial deseada y en el tiempo final sea la posición final deseada, vamos a llamar a nuestra función $\theta(t)$, por lo tanto requerimos:
$\theta(0)=\theta_{0}$
$\theta(t_{f})=\theta_{f}$
Otro requerimiento es que la función sea suave, osea que no posea discontinuidades en velocidad. Ello se traduce en las siguientes condiciones:
$\dot{\theta}(0)=0$
$\dot{\theta}(t_{f})=0$
Estas restricciones pueden satisfacerce con un polinomio de por lo menos grado tres. O sea :
$\theta(t)=a_{0}+a_{1}t+a_{2}t^{2}+a_{3}t^{3}$
Por ello, la velocidad y la aceleración serán:
$\dot{\theta}(t)=a_{1}+2a_{2}t+3a_{3}t^{2}$
$\ddot{\theta}(t)=2a_{2}+6a_{3}t$
Si combinamos las ecuaciones anteriores y resolvemos para los parámetros del polinomio $a_{i}$ nos queda que:
$a_{0}=\theta_{0}$
$a_{1}=0$
$a_{2}=\frac{3}{t_{f}^{2}}(\theta_{f}-\theta_{0})$
$a_{3}=-\frac{2}{t_{f}^{3}}(\theta_{f}-\theta_{0})$
A continuación realizamos las implementaciones que resuelven el ejercicio
In [2]:
import numpy as np
In [3]:
def get_param(theta_0, theta_f,t_f):
"""
Funcion para obtener los coeficientes de un polinomio de grado tres
tal que represente al movimiento de un link, con el requerimiento de
que su movimiento sea suave.
Imputs:
theta_0 : angulo inicial de la articulacion
theta_f : angulo final de la articulacion
t_f : tiempo total deseado para el movimiento
Outputs
coeficientes del polinomio
a_0+a_1t + a_2t^2+a_3t^3
"""
a_0 = float(theta_0)
a_1 = 0.
a_2 = (3./(t_f**2))*(theta_f - theta_0)
a_3 = (-2./(t_f**3))*(theta_f - theta_0)
return a_0, a_1, a_2, a_3
Graficamos la posición ($\theta(t)$), velocidad($\dot{\theta}(t)$), aceleración($\ddot{\theta}(t)$)
In [4]:
%pylab inline
#ajustamos el tamaño de la imagen
plt.rcParams['figure.figsize'] = 10,8
t_f = 4
theta_0 = -5
theta_f = 80
t = np.linspace(0,t_f)
#calculamos los parametros
a_0,a_1,a_2,a_3 = get_param(theta_0,theta_f,t_f)
#generamos las funciones que vamos a graficar
posicion = lambda t : a_0 + a_1*t + a_2*t**2 + a_3*t**3
velocidad = lambda t : a_1 + 2*a_2*t + 3*a_3*t**2
aceleracion = lambda t : 2*a_2 + 6*a_3*t
In [5]:
plt.plot(t,posicion(t))
plt.title('Posicion de la articulacion',fontsize=25)
plt.xlabel(r'$t$',fontsize=20)
plt.ylabel(r'$\theta(t)$',fontsize=20)
plt.grid()
plt.show()
In [6]:
plt.plot(t,velocidad(t))
plt.title('Velocidad de la articulacion',fontsize=25)
plt.xlabel(r'$t$',fontsize=20)
plt.ylabel(r'$\dot{\theta}(t)$',fontsize=20)
plt.grid()
plt.show()
In [7]:
plt.plot(t, aceleracion(t))
plt.title('Aceleracion de la articulacion',fontsize=25)
plt.xlabel(r'$t$',fontsize=20)
plt.ylabel(r'$\ddot{\theta}(t)$',fontsize=20)
plt.grid()
plt.show()
Una alternativa para generar la trayectoria sería interpolar linealmente entre los dos puntos deseados. Esta alternativa debe ser modificada ya que haría que las velocidades inicial y final sean discontinuas, para salvar este problema se recurre a "empalmar" estas regiones con otras funciones tal que su derivadas(velocidad) sean continuas.
Supongamos que las mezclas parabólicas tienen la misma duración, por lo tanto se utiliza la misma aceleración constante durante ambas mezclas, sea $t_{h}$ el tiempo intermedio de duración y $\theta_{h}$ el punto intermedio en la posición. La velocidad al final de la región de mezcla debe ser igual a la velocidad en la region lineal por lo que tenemos que:
$\ddot{\theta}t_{b}=\frac{\theta_{h}-\theta_{b}}{t_{h}-t_{b}}$
En donde $\theta_{b}$ es el valor de $\theta$ al final de la región de mezcla y $\ddot{\theta}$ es la aceleración que actúa durante la región de mezcla. El valor de $\theta_{b}$ se obtiene mediante:
$\theta_{b}=\theta_{0}+\frac{1}{2}\ddot{\theta}t_{b}^{2}$
Al combinar las ecuaciones y hacer que el tiempo total sea $t=2t_{h}$, obtenemos:
$\ddot{\theta}t_{b}^{2}-\ddot{\theta}t t_{b}+(\theta_{f}-\theta_{0})=0$
Resolviendo para $t_{b}$
$t_{b}=\frac{t}{2}-\frac{\sqrt{\ddot{\theta}t^{2}-4\ddot{\theta}(\theta_{f}-\theta_{0})}}{2\ddot{\theta}}$
Luego podemos saber cual es la restricción sobre la aceleración utilizada en la mezcla:
$\ddot{\theta} \geq \frac{4(\theta_{f}-\theta_{0})}{t^{2}}$
En la igualdad la región lineal se reduce a cero, y cuando la aceleración es $\infty$ la región se transforma en solo una región lineal.
In [8]:
t_0 = 0
t_f = 4.
q_0 = -5
q_f = 80
V_min = (q_f - q_0)/t_f
V = V_min + 10
t_b = (q_0 - q_f + V*t_f)/V #tiempo de blend
a_1 = q_0
a_2 = 0.
a_3 = V/(2*t_b)
b_1 = q_f - (V*t_f**2)/(2*t_b)
b_2 = V*t_f/t_b
b_3 = - V/(2*t_b)
#formamos las nuevas funciones notese que no se utilizan millones de if :)
pos_blend = lambda t:(a_1 + a_2*t + a_3*t**2)*(t<=t_b)+((q_f+q_0-V*t_f)/2+V*t )*((t>t_b)-(t>=(t_f-t_b)))+(b_1+b_2*t+b_3*t**2)*(t>(t_f-t_b))
vel_blend = lambda t: (a_2 + 2*a_3*t)*(t<=t_b)+V*((t>t_b)-(t>=(t_f-t_b )))+(b_2+2*b_3*t)*(t>(t_f-t_b))
acel_blend = lambda t: (2*a_3)*(t<=t_b) + 0*((t>t_b)-(t>=(t_f-t_b ))) + 2*b_3* (t>(t_f-t_b))
In [9]:
t = np.linspace(t_0, t_f)
In [10]:
plt.plot(t,pos_blend(t))
plt.plot(t_b,pos_blend(t_b),'ro')
plt.title('Posicion', fontsize=25)
plt.xlabel(r'$t$', fontsize=20)
plt.ylabel(r'$\theta(t)$',fontsize=20)
plt.grid()
plt.show()
In [11]:
plt.plot(t,vel_blend(t))
plt.title('Velocidad', fontsize=25)
plt.xlabel(r'$t$', fontsize=20)
plt.ylabel(r'$\dot{\theta}(t)$',fontsize=20)
plt.grid()
plt.show()
In [12]:
plt.plot(t,acel_blend(t))
plt.title('Aceleracion', fontsize=25)
plt.xlabel(r'$t$', fontsize=20)
plt.ylabel(r'$\ddot{\theta}(t)$',fontsize=20)
plt.grid()
plt.show()
En este caso tenemos dos polinomios los cuales se unen en un punto especificado.
$\theta_{1}(t)=a_{10}+a_{11}t+a_{12}t^{2}+a_{13}t^{3}$
$\theta_{2}(t)=a_{20}+a_{21}t+a_{22}t^{2}+a_{23}t^{3}$
Cada polinomio cúbico se evaluará sobre un intervalo que empieza en $t=0$ y termina en $t=t_{fi}$, en donde $i=1,2$
Las restricciones que queremos hacer valer son:
$\theta_{0}=a_{10}$
$\theta_{v}=a_{10}+a_{11}t_{f1}+a_{12}t_{f1}^{2}+a_{13}t_{f1}^{3}$
$\theta_{v}=a_{20}$
$\theta_{g}=a_{20}+a_{21}t_{f2}+a_{22}t_{f2}^{2}+a_{23}t_{f2}^{3}$
$0=a_{11}$
$0 = a_{21} + 2a_{22}t_{f2}+3a_{23}t_{f2}^{2} $
$a_{11}+2a_{12}t_{f1}+3a_{13}t_{f1}^{2} = a_{12}$
$2a_{12}+6a_{13}t_{f1} = 2a_{22}$
Estas restricciones especifican un problema de ecuaciones lineales en el cual hay ocho ecuaciones y ocho variables desconocidas. Al resolver para el caso $t_{f}=t_{f1}=t_{f2}$ obtenemos:
$a_{10} = \theta_{0}$
$a_{11} = 0$
$a_{12} = \frac{12 \theta_{v}-3\theta_{g}-9\theta_{0}}{4t_{f}^{2}}$
$a_{13} = \frac{-8\theta_{v}+3\theta_{g}+5\theta_{0}}{4t_{f}^{3}}$
$a_{20} = \theta_{v}$
$a_{21} = \frac{3\theta_{g}-3\theta_{0}}{4t_{f}}$
$a_{22} = \frac{-12\theta_{v}+6\theta_{g}+6\theta_{0}}{4t_{f}^{2}}$
$a_{23} = \frac{8\theta_{v}-5\theta_{g}-3\theta_{0}}{4t_{f}^{3}}$
A continuación realizamos la implementación y resolución del ejercicio:
In [13]:
def get_param_2(theta_0,theta_v, theta_g, t_f):
"""
"""
a_10 = float(theta_0)
a_11 = 0.
a_12 = (12*theta_v - 3*theta_g - 9*theta_0)/(4*t_f**2)
a_13 = (-8*theta_v + 3*theta_g + 5*theta_0)/(4*t_f**3)
a_20 = theta_v
a_21 = (3*theta_g - 3*theta_0)/(4*t_f)
a_22 = (-12*theta_v + 6*theta_g + 6*theta_0)/(4*t_f**2)
a_23 = (8*theta_v - 5*theta_g - 3*theta_0)/(4*t_f**3)
return a_10, a_11, a_12, a_13, a_20, a_21, a_22, a_23
In [14]:
theta_0 = 5.
theta_v = 15.
theta_g = -10.
t_f = 2.
a_10, a_11, a_12, a_13, a_20, a_21, a_22, a_23 = get_param_2(theta_0,theta_v,theta_g,t_f)
pos_1 = lambda t : (a_10 + a_11*t + a_12*t**2 + a_13*t**3)
pos_2 = lambda t : (a_20 + a_21*t + a_22*t**2 + a_23*t**3)
In [15]:
t1 = np.linspace(0,t_f)
t2 = np.linspace(t_f,2*t_f)
In [16]:
plt.subplot(122)
plt.title('Primer Segmento')
plt.plot(t1,pos_1(t1))
plt.xlabel(r'$t$', fontsize=20)
plt.grid()
plt.subplot(121)
plt.title('Segundo segmento')
plt.xlabel(r'$t$', fontsize=20)
plt.plot(t1,pos_2(t1))
plt.grid()
plt.show()
En este caso tenemos dos segmentos lineales con mezclas parabólicas que se unen mediante tres puntos via. Graficamente sería:
In [17]:
from IPython.core.display import Image
Image(filename='Imagenes/blend_parbolic_via_points.png')
Out[17]:
El primer segmento se resuelve para $t_{1}$ igualando las dos expresiones para la velocidad durante la fase lineal del segmento:
$\frac{\theta_{2}-\theta_{1}}{t_{12}-\frac{1}{2}t_{1}}=\ddot{\theta}_{1}t_{1}$
Esta se puede resolver fácilmente para $t_{1}$, el tiempo de mezcla en el punto inicial; despues $\dot{\theta}_{12}$ y $t_{12}$ luego:
$\ddot{\theta}_{1}=sign(\theta_{2}-\theta_{1})|\ddot{\theta}_{1}|$
$t_{1} = t_{d_{12}} - \sqrt{t_{d_{12}}^{2}-\frac{2(\theta_{2}-\theta_{1})}{\ddot{\theta}_{1}}}$
$\dot{\theta}_{12}=\frac{\theta_{2}-\theta_{1}}{t_{d_{12}}-\frac{1}{2}t_{1}}$
$t_{12} = t_{d_{12}}- t_{1}-\frac{1}{2}t_{2}$
De igual forma para el último segmento(el que conecta los puntos $(n-1)$ y $n$) tenemos que:
$\frac{\theta_{n-1}-\theta_{n}}{t_{(n-1)n}-\frac{1}{2}t_{n}}=\ddot{\theta}_{n}t_{n}$
La cual nos lleva a la solución:
$\ddot{\theta}_{n}=sign(\theta_{n-1}-\theta_{n})|\ddot{\theta}_{n}|$
$t_{n} = t_{d_{(n-1)n}} - \sqrt{t_{d_{(n-1)n}}^{2}-\frac{2(\theta_{n}-\theta_{n-1})}{\ddot{\theta}_{n}}}$
$\dot{\theta}_{(n-1)n}=\frac{\theta_{n-1}-\theta_{n}}{t_{d_{(n-1)n}}-\frac{1}{2}t_{n}}$
$t_{(n-1)n} = t_{d_{(n-1)n}}- t_{n}-\frac{1}{2}t_{n-1}$
haciendo los cálculos llegamos a :
$t_{d_{12}}=t_{d_{23}}=2$
$\ddot{\theta}=60$
$\ddot{\theta_{1}}= sgn(15-5)|60|=60$
$t_{1}=2-\sqrt{4-\frac{20}{60}} =2-\sqrt{\frac{11}{3}}$
$\dot{\theta_{12}}=\frac{10}{2-\frac{1}{2}(2-\sqrt{\frac{11}{3}})}=\frac{20}{2+\sqrt{\frac{11}{3}}}$
$\ddot{\theta_{3}}= sgn(15-(-10))|60|=60$
$t_{3}=2-\sqrt{4+\frac{50}{60}}=2-\sqrt{\frac{19}{6}}$
$\dot{\theta}_{23}=\frac{-25}{2-\frac{1}{2}(2-\frac{\sqrt{19}}{6})}=\frac{-50}{2+\sqrt{\frac{19}{6}}}$
$t_{2} = \frac{\dot{\theta}_{23}-\dot{\theta}_{12}}{-60}$
In [17]: