Martín Noblía

Tp4

Control de Robots 2013

Ingeniería en Automatización y Control

Universidad Nacional de Quilmes

Ejercicio 1

En la figura se muestra un manipulador de dos vínculos con articulaciones giratorias. Calcule la velocidad de la punta del brazo como función de las velocidades de articulación. Dar respuesta en términos del frame {3}, y del frame {0}.


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]:
$$\left[\begin{smallmatrix}L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )}\\L_{1} \sin{\left (\theta_{1} \right )} + L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )}\end{smallmatrix}\right]$$

In [8]:
#creamos un vector con las variables 
Theta = Matrix([theta_1,theta_2])
Theta


Out[8]:
$$\left[\begin{smallmatrix}\theta_{1}\\\theta_{2}\end{smallmatrix}\right]$$

In [9]:
#calculamos el jacobiano(con un metodo) respecto al vector de variables
J = M.jacobian(Theta)
J


Out[9]:
$$\left[\begin{smallmatrix}- L_{1} \sin{\left (\theta_{1} \right )} - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )} & - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )}\\L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )} & L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )}\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}\theta_{p 1}\\\theta_{p 2}\end{smallmatrix}\right]$$

In [11]:
X_p = J * Theta_punto
X_p


Out[11]:
$$\left[\begin{smallmatrix}- L_{2} \theta_{p 2} \sin{\left (\theta_{1} + \theta_{2} \right )} + \theta_{p 1} \left(- L_{1} \sin{\left (\theta_{1} \right )} - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )}\right)\\L_{2} \theta_{p 2} \cos{\left (\theta_{1} + \theta_{2} \right )} + \theta_{p 1} \left(L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )}\right)\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}\cos{\left (\theta_{1} \right )} & - \sin{\left (\theta_{1} \right )} & 0\\\sin{\left (\theta_{1} \right )} & \cos{\left (\theta_{1} \right )} & 0\\0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}\cos{\left (\theta_{2} \right )} & - \sin{\left (\theta_{2} \right )} & 0\\\sin{\left (\theta_{2} \right )} & \cos{\left (\theta_{2} \right )} & 0\\0 & 0 & 1\end{smallmatrix}\right]$$

In [14]:
R_2_3 = Matrix([[1, 0,0],[0,1,0],[0,0,1]])
R_2_3


Out[14]:
$$\left[\begin{smallmatrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}0\\0\\\theta_{p 1}\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}0\\0\\0\end{smallmatrix}\right]$$

In [17]:
omega_2_2 = R_1_2.T *(omega_1_1) + theta_p_2 * Z
omega_2_2


Out[17]:
$$\left[\begin{smallmatrix}0\\0\\\theta_{p 1} + \theta_{p 2}\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}L_{1} \theta_{p 1} \sin{\left (\theta_{2} \right )}\\L_{1} \theta_{p 1} \cos{\left (\theta_{2} \right )}\\0\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}L_{1} \theta_{p 1} \sin{\left (\theta_{2} \right )}\\L_{1} \theta_{p 1} \cos{\left (\theta_{2} \right )} + L_{2} \left(\theta_{p 1} + \theta_{p 2}\right)\\0\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}L_{1} \sin{\left (\theta_{2} \right )} & 0\\L_{1} \cos{\left (\theta_{2} \right )} + L_{2} & L_{2}\end{smallmatrix}\right]$$

Ejercicio 2

Encontrar el Jacobiano del manipulador de 3 grados de libertad siguiente. Escribirlo en términos del frame {4}, cuyo origen está en el extremo de la herramienta y tiene igual orientación que el frame {3}.


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]:
$$\left[\begin{smallmatrix}L_{3} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + \left(L_{1} + L_{2} \cos{\left (\theta_{2} \right )}\right) \cos{\left (\theta_{1} \right )}\\L_{3} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + \left(L_{1} + L_{2} \cos{\left (\theta_{2} \right )}\right) \sin{\left (\theta_{1} \right )}\\0\end{smallmatrix}\right]$$

In [23]:
Theta_2 = Matrix([theta_1,theta_2,theta_3])
J2 = P_0_ORG.jacobian(Theta_2)
J2


Out[23]:
$$\left[\begin{smallmatrix}- L_{3} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} - \left(L_{1} + L_{2} \cos{\left (\theta_{2} \right )}\right) \sin{\left (\theta_{1} \right )} & - L_{2} \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )} - L_{3} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} & - L_{3} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )}\\L_{3} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + \left(L_{1} + L_{2} \cos{\left (\theta_{2} \right )}\right) \cos{\left (\theta_{1} \right )} & - L_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )} - L_{3} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )} & - L_{3} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )}\\0 & 0 & 0\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}\cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} & - \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} & \sin{\left (\theta_{1} \right )}\\\sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} & - \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )} & - \cos{\left (\theta_{1} \right )}\\0 & 0 & 1\end{smallmatrix}\right]$$

In [25]:
J2_4 = R_0_4*J2

J2_4


Out[25]:
$$\left[\begin{smallmatrix}\left(- L_{3} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} - \left(L_{1} + L_{2} \cos{\left (\theta_{2} \right )}\right) \sin{\left (\theta_{1} \right )}\right) \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} - \left(L_{3} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + \left(L_{1} + L_{2} \cos{\left (\theta_{2} \right )}\right) \cos{\left (\theta_{1} \right )}\right) \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} & - \left(- L_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )} - L_{3} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} + \left(- L_{2} \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )} - L_{3} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )}\right) \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} & L_{3} \sin{\left (\theta_{1} \right )} \sin^{2}{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} - L_{3} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos^{2}{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )}\\\left(- L_{3} \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} - \left(L_{1} + L_{2} \cos{\left (\theta_{2} \right )}\right) \sin{\left (\theta_{1} \right )}\right) \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} - \left(L_{3} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} + \left(L_{1} + L_{2} \cos{\left (\theta_{2} \right )}\right) \cos{\left (\theta_{1} \right )}\right) \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )} & - \left(- L_{2} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} \right )} - L_{3} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )}\right) \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )} + \left(- L_{2} \sin{\left (\theta_{2} \right )} \cos{\left (\theta_{1} \right )} - L_{3} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )}\right) \sin{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )} & L_{3} \sin^{2}{\left (\theta_{1} \right )} \sin^{2}{\left (\theta_{2} + \theta_{3} \right )} - L_{3} \sin{\left (\theta_{1} \right )} \sin{\left (\theta_{2} + \theta_{3} \right )} \cos{\left (\theta_{1} \right )} \cos{\left (\theta_{2} + \theta_{3} \right )}\\0 & 0 & 0\end{smallmatrix}\right]$$

Ejercicio 3

El manipulador de la figura siguiente, mueve su extremo a lo largo del eje $X$ a una velocidad de 1.0 m/seg. Encontrar el Jacobiano y analizar la singularidad para $\theta_{2}=0º$


In [26]:
#Determinante del Jacobiano
J_0_3.det()


Out[26]:
$$L_{1} L_{2} \sin{\left (\theta_{2} \right )}$$

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]:
$$\left[\begin{smallmatrix}\frac{1}{L_{1} \sin{\left (\theta_{2} \right )}} & 0\\- \frac{L_{1} \cos{\left (\theta_{2} \right )} + L_{2}}{L_{1} L_{2} \sin{\left (\theta_{2} \right )}} & \frac{1}{L_{2}}\end{smallmatrix}\right]$$

Ejercicio 4

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]:
$$\left[\begin{smallmatrix}- L_{1} \sin{\left (\theta_{1} \right )} - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )} - L_{3} \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & - L_{2} \sin{\left (\theta_{1} + \theta_{2} \right )} - L_{3} \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & - L_{3} \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\\L_{1} \cos{\left (\theta_{1} \right )} + L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )} + L_{3} \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & L_{2} \cos{\left (\theta_{1} + \theta_{2} \right )} + L_{3} \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & L_{3} \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\\1 & 1 & 1\end{smallmatrix}\right]$$

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]:
$$\left[\begin{smallmatrix}- 4 \sin{\left (\theta_{1} \right )} - 3 \sin{\left (\theta_{1} + \theta_{2} \right )} - 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & - 3 \sin{\left (\theta_{1} + \theta_{2} \right )} - 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & - 2 \sin{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\\4 \cos{\left (\theta_{1} \right )} + 3 \cos{\left (\theta_{1} + \theta_{2} \right )} + 2 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & 3 \cos{\left (\theta_{1} + \theta_{2} \right )} + 2 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )} & 2 \cos{\left (\theta_{1} + \theta_{2} + \theta_{3} \right )}\\1 & 1 & 1\end{smallmatrix}\right]$$

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]:
 [[-1.77417834 -1.37484468 -0.77883668]
  [ 8.76233838  4.78232172  1.84212199]
 [ 1.          1.          1.        ]]

In [36]:
f2 = lambdify((theta_1,theta_2,theta_3),J_real_inv,'numpy')

In [37]:
f2(.1,np.pi/2,1)


Out[37]:
 [[-0.02495835  0.24875104  0.42073549]
  [-0.3067097  -0.28202885 -0.78093703]
 [ 0.33166806  0.03327781  1.36020154]]

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


Populating the interactive namespace from numpy and matplotlib
WARNING: pylab import has clobbered these variables: ['Polygon', 'poly', 'sign', 'flatten', 'conjugate', 'diff', 'Circle', 'tan', 'roots', 'plot', 'eye', 'log', 'floor', 'diag', 'invert', 'var', 'nan', 'sqrt', 'source', 'add', 'zeros', 'take', 'prod', 'plotting', 'product', 'power', 'multinomial', 'transpose', 'test', 'beta', 'ones', 'sinh', 'vectorize', 'cosh', 'mod', 'trunc', 'cos', 'seterr', 'tanh', 'pi', 'sin', 'binomial', 'solve', 'exp', 'reshape', 'gamma', 'interactive']
`%pylab --no-import-all` prevents importing * from pylab and numpy

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]: