In [1]:
from IPython.core.display import Image
In [2]:
Image(filename='Imagenes/copy_left.png')
Out[2]:
In [3]:
Image(filename='Imagenes/robot1_tp4.png')
Out[3]:
Como sabemos podemos encontrar la expresión del par ordenado ($x,y$) de la muñeca del robot, como:
$x = L_1 cos(\theta_1) + L_2 cos(\theta_1 + \theta_2)$
$y = L_1 sin(\theta_1) + L_2 sin(\theta_1 + \theta_2)$
In [4]:
from sympy import *
#import numpy as np
#%pylab inline
In [5]:
#Con esto las salidas van a ser en LaTeX
init_printing(use_latex=True)
In [6]:
#estos son simbolos especiales que los toma como letras griegas directamente(muuy groso)
theta_1, theta_2, theta_3, L_1, L_2, L_3 = symbols('theta_1, theta_2 theta_3 L_1 L_2 L_3')
In [7]:
#Creamos un vector donde estan las parametrizaciones de x e y
M = Matrix([L_1*cos(theta_1)+ L_2*cos(theta_1+theta_2),L_1*sin(theta_1)+L_2*sin(theta_1+theta_2)])
M
Out[7]:
In [8]:
#creamos un vector con las variables
Theta = Matrix([theta_1,theta_2])
Theta
Out[8]:
In [9]:
#calculamos el jacobiano(con un metodo) respecto al vector de variables
J = M.jacobian(Theta)
J
Out[9]:
Luego como sabemos la velocidades se calculan de acuerdo a la formula:
$\dot{x} = J(\theta)\dot{\theta}$
Más precisamente en este caso tenemos:
$^{0}\dot{x} = (^{0}J(\theta))\dot{\theta}$
osea el vector velocidad que obtengamos va a estar expresado en el frame {0}(por como parametrizamos $(x,y)$)
In [10]:
theta_p_1, theta_p_2 = symbols('theta_p_1 theta_p_2') #velocidades angulares
Theta_punto = Matrix([theta_p_1, theta_p_2])
Theta_punto
Out[10]:
In [11]:
X_p = J * Theta_punto
X_p
Out[11]:
Luego para obtener la velocidad en expresada en el frame {3} propagamos desde el frame {0} de acuerdo a las siguientes ecuaciones:
$ ^{i+1} \omega _{i+1} = (^{i+1}_{i}R)(\omega _{i}) + (\dot{\theta}_{i+1})(^{i+1} Z_{i+1}) $
$^{i+1} v_{i+1} = (^{i+1}_{i}R)(^{i}v_{i} + (^{i} \omega_{i})\times(^{i}P_{i+1}) )$
Como vemos necesitamos las transformaciones de rotación $(^{i+1}_{i}R)$ con $i = 0,1,2,3$ las cuales las extraemos de las transformaciones de vínculo:
$$^{0}_{1}T = \begin{bmatrix} c_{1} & -s_{1} & 0 & 0 \\\\ s_{1} & c_{1} & 0 & 0 \\\\ 0 & 0 & 1 & 0 \\\\ 0 & 0 & 0 & 1 \end{bmatrix}$$$$^{1}_{2}T = \begin{bmatrix} c_{2} & -s_{2} & 0 & L_{1} \\\\ s_{2} & c_{2} & 0 & 0 \\\\ 0 & 0 & 1 & 0 \\\\ 0 & 0 & 0 & 1 \end{bmatrix}$$$$^{2}_{3}T = \begin{bmatrix} 1 & 0 & 0 & L_{2} \\\\ 0 & 1 & 0 & 0 \\\\ 0 & 0 & 1 & 0 \\\\ 0 & 0 & 0 & 1 \end{bmatrix}$$
In [12]:
R_0_1 = Matrix([[cos(theta_1), -sin(theta_1),0],[sin(theta_1),cos(theta_1),0],[0,0,1]])
R_0_1
Out[12]:
In [13]:
R_1_2 = Matrix([[cos(theta_2), -sin(theta_2),0],[sin(theta_2),cos(theta_2),0],[0,0,1]])
R_1_2
Out[13]:
In [14]:
R_2_3 = Matrix([[1, 0,0],[0,1,0],[0,0,1]])
R_2_3
Out[14]:
vamos a nombrar a las variables según la siguiente convención:
$^{i}\omega_{i+1}$ = omega_i_i+1
$^{i}v_{i+1}$ = v_i_i+1
In [15]:
Z = Matrix([0,0,1])
omega_0_0 = Matrix([0,0,0])
v_0_0 = Matrix([0,0,0])
omega_1_1 = R_0_1.T * omega_0_0 + theta_p_1 * Z
omega_1_1
Out[15]:
In [16]:
P_0_1 = Matrix([0,0,0])
v_1_1 = R_0_1.T *(v_0_0 + omega_0_0.cross(P_0_1).T)
v_1_1
Out[16]:
In [17]:
omega_2_2 = R_1_2.T *(omega_1_1) + theta_p_2 * Z
omega_2_2
Out[17]:
In [18]:
P_1_2 = Matrix([L_1,0,0])
v_2_2 = R_1_2.T*(v_1_1 + omega_1_1.cross(P_1_2).T)
v_2_2
Out[18]:
Se puede observar que $^{3}\omega_3$=$^{2}\omega_2$
In [19]:
omega_3_3 = omega_2_2
P_2_3 = Matrix([L_2,0,0])
v_3_3 = R_2_3.T*(v_2_2 + omega_2_2.cross(P_2_3).T)
v_3_3
Out[19]:
Vemos que el jacobiano expresado en el frame {3} queda expresado:
In [20]:
J_0_3 = Matrix([[L_1*sin(theta_2),0],[L_1*cos(theta_2)+L_2,L_2]])
J_0_3
Out[20]:
In [21]:
Image(filename='Imagenes/dibujo_robot2_tp2.png')
Out[21]:
Primero hallamos a $(^{0}_{4}T)=(^{0}_{3}T)(^{3}_{4}T)$. Donde:
$$^{3}_{4}T = \begin{bmatrix} 1 & 0 & 0 & L_{3} \\\\ 0 & 1 & 0 & 0 \\\\ 0 & 0 & 1 & 0 \\\\ 0 & 0 & 0 & 1 \end{bmatrix}$$
In [22]:
P_0_ORG = Matrix([cos(theta_1)*(L_1 + L_2*cos(theta_2))+L_3*cos(theta_2+theta_3)*cos(theta_1),sin(theta_1)*(L_1+L_2*cos(theta_2))+L_3*cos(theta_2+theta_3)*sin(theta_1),0])
P_0_ORG
Out[22]:
In [23]:
Theta_2 = Matrix([theta_1,theta_2,theta_3])
J2 = P_0_ORG.jacobian(Theta_2)
J2
Out[23]:
Ahora lo que resta es cambiar de coordenadas: $(^{4}_{0}R)(^{0}J(\theta))$
In [24]:
R_0_4 = Matrix([[cos(theta_2+theta_3)*cos(theta_1),-sin(theta_2+theta_3)*cos(theta_1),sin(theta_1)],[cos(theta_2+theta_3)*sin(theta_1),-sin(theta_2+theta_3)*sin(theta_1),-cos(theta_1)],[0,0,1]])
R_0_4
Out[24]:
In [25]:
J2_4 = R_0_4*J2
J2_4
Out[25]:
In [26]:
#Determinante del Jacobiano
J_0_3.det()
Out[26]:
Vemos que el determinante del Jacobiano se anula en los $\theta_{2}=k \pi$, con $k=0 \pm 1 , \pm 2 ....$
Si calculamos la inversa se hace evidente que a medida que la articulación se acerca a el valor de singularidad la velocidad aumenta y tiende a $\infty$
$ \lim_{\theta_{2} \to k \pi}(\mathbf{\theta})=+\infty $ Ya que como sabemos:
$(J(\theta))^{-1}\dot{x} = \dot{\theta}$
In [27]:
J_0_3.inv()
Out[27]:
En este ejercicio vamos a desarrollar el algoritmo de control de velocidad del siguiente robot:
In [28]:
Image(filename='Imagenes/robot1_tp2_300.png')
Out[28]:
Este algoritmo se basa en el siguiente articulo:
D.E. Whitney, "Resolved Motion Rate Control of Manipulators and Human Prostheses," IEEE Transactions on Man-Machine Systems,1969.
Y puede ser resumido en el siguiente diagrama de bloques:
In [29]:
Image(filename='Imagenes/resolved_rate.png')
Out[29]:
Donde:
$\dot{X}_{C}$ : Velocidad de articulación comandada
$\dot{\Theta}_{C}$ : Velocidades cartesianas comandadas
$\Theta_{C}$ : Ángulo de articulación comandado
$\Theta_{A}$ : Ángulo de articulación alcanzado
El jacobiano de este manipulador expresado en la trama cero es:
In [30]:
#jacobiano simbolico
a11 = -L_1*sin(theta_1) - L_2*sin(theta_1+theta_2) - L_3*sin(theta_1+theta_2+theta_3)
a12 = -L_2*sin(theta_1+theta_2) - L_3*sin(theta_1+theta_2+theta_3)
a13 = -L_3*sin(theta_1+theta_2+theta_3)
a21 = L_1*cos(theta_1) + L_2*cos(theta_1+theta_2) + L_3*cos(theta_1+theta_2+theta_3)
a22 = L_2*cos(theta_1+theta_2) + L_3*cos(theta_1+theta_2+theta_3)
a23 = L_3*cos(theta_1+theta_2+theta_3)
a31 = 1
a32 = 1
a33 = 1
J = Matrix([[a11,a12,a13],[a21,a22,a23],[a31,a32,a33]])
J
Out[30]:
Luego reemplazamos las longitudes de los links
In [31]:
#reemplazando las longitudes de los links
J_real = J.subs([(L_1,4),(L_2,3),(L_3,2)])
J_real
Out[31]:
A continuación hallamos la expresión simbólica para la inversa del Jacobiano
In [32]:
J_real_inv = J_real.inv()
In [33]:
import numpy as np
import matplotlib.pyplot as plt
In [34]:
#creamos una funcion numerica a partir de la simbolica
f1 = lambdify((theta_1,theta_2,theta_3),J_real,'numpy')
In [35]:
#probamos si funciona
f1(.1,.1,.2)
Out[35]:
In [36]:
f2 = lambdify((theta_1,theta_2,theta_3),J_real_inv,'numpy')
In [37]:
f2(.1,np.pi/2,1)
Out[37]:
Luego definimos el vector de velocidades que se nos da en el enunciado:
$X = [\dot{x},\dot{y},\omega_{z}]=[0.2,-0.3,-0.2]$
In [38]:
X = np.array([.2,-.3,-.2]).T
Theta_vec = np.zeros((3,50))
Theta_vec_0 = np.array([10,20,30]).T * (np.pi/180)
Theta_vec[:,0] = Theta_vec_0
Theta_vec_punto = np.zeros_like(Theta_vec)
determinante = np.zeros(50)
en el enunciado se propone el siguiente esquema de integración:
$\Theta_{new} = \Theta_{old} + \dot{\Theta} \Delta t$
Donde $\Delta t = 0.1(seg)$
y el tiempo total de simulación es $t_{total}=5$
In [39]:
dt = .1
t_tot = 5
N_muestras = t_tot/dt
A continuación la implementación del lazo:
In [40]:
#definimos una funcion para
def jacobian_inv(vec):
"""
Funcion para evaluar el jacobiano inverso
Input:
vec: Vector donde estan guardados los valores de los angulos
Output:
M: Jacobiano inverso (ndarray (3,3) )
"""
q_1 = vec[0]
q_2 = vec[1]
q_3 = vec[2]
M = f2(q_1, q_2, q_3)
return np.asarray(M)
In [41]:
def jacobian(vec):
"""
Funcion para evaluar el jacobiano
Input:
vec: Vector donde estan guardados los valores de los angulos
Output:
M: Jacobiano (ndarray (3,3) )
"""
q_1 = vec[0]
q_2 = vec[1]
q_3 = vec[2]
M = f1(q_1, q_2, q_3)
return np.asarray(M)
In [42]:
for i in xrange(int(N_muestras)-1):
aux = jacobian_inv(Theta_vec[:,i])
aux2 = jacobian(Theta_vec[:,i])
determinante[i] = np.linalg.det(aux2)
Theta_vec_punto[:,i+1] = np.dot(aux,X)
Theta_vec[:,i+1] = Theta_vec[:,i] + Theta_vec_punto[:,i+1]*dt
Graficamos las velocidades $\dot{\Theta}$
In [43]:
%pylab inline
plt.rcParams['figure.figsize'] = 10,8
plt.plot(Theta_vec_punto[0,:],'ro')
plt.plot(Theta_vec_punto[1,:],'go')
plt.plot(Theta_vec_punto[2,:],'ko')
plt.title('velocidades angulares de articulacion',fontsize=17)
plt.xlabel('$t$',fontsize=30)
plt.ylabel('$\dot{\Theta}$',fontsize=30)
plt.legend([r'$\dot{\theta_{1}}$',r'$\dot{\theta_{2}}$',r'$\dot{\theta_{3}}$'],fontsize=20)
plt.show()
Graficamos los angulos de articulacion $\Theta$ en funcion del tiempo
In [44]:
plt.plot(Theta_vec[0,:],'ro')
plt.plot(Theta_vec[1,:],'go')
plt.plot(Theta_vec[2,:],'ko')
plt.title('angulos de articulacion',fontsize=20)
plt.legend([r'$\theta_{1}$',r'$\theta_{2}$',r'$\theta_{3}$'],fontsize=17)
plt.xlabel('$t$',fontsize=30)
plt.ylabel('$\Theta$',fontsize=30)
plt.show()
In [45]:
plt.plot(determinante,'yo')
plt.title('Determinante',fontsize=20)
plt.xlabel('$t$',fontsize=30)
plt.show()
In [45]: