Martn Nobla

Tp6

Control de Robots 2013

Ingeniera en Automatizacin y Control

Universidad Nacional de Quilmes

Ejercicio 1

Este ejercicio se enfoca en el anlisis de dinmica inversa para el robot planar 2-DOF 2R. Este robot

constituye las primeras dos articulaciones R y los primeros dos vnculos mviles del robot planar 3-DOF

3R.

Para el robot planar 2R, calcule los momentos de torsin de artuculaciones requeridas(es decir, resuelva

el problema de dinmica inversa ) para proporcionar el movimiento comandado en cada intervalo, en un

esquema de control con velocidad resuelta. Puede usar el mtodo dinmico de Lagrange.

Dadas :

, ambos vnculos son de acero slido con una densidad de masa

ambos tienen las dimensiones de anchura y grosor . Se supone que las

articulaciones angulares son perfectas, conectando los vnculos precisamente en sus extremos(lo que no es

fisicamente posible )

Los ngulos iniciales son :

La velocidad cartesiana comandada(constante) es :

Simule el movimiento en , con un intervalo de tiempo de control de . Presente cinco

trazos(cada conjunto en un grfico separado ) :

Los dos ngulos de articulacin (grados) contra el tiempo

Las dos velociadades de articulacin

Las dos aceleraciones de articulacin de , (rad)

Los dos momentos de torsin de articulacin de dinmica inversa contra el

tiempo

Realice la simulacin dos veces. La primera ignore la gravedad(el plano de movimiento es normal al efecto de la

gravedad ); la segunda vez considere la gravedad en la direccin negativa de

Comenzamos la resolucin planteando y asignando todas las constantes y datos del problema:

In [169]: import numpy as np

= 1.0(m)L1 = 0.5(m)L2 = 7806(kg/ )m3 = t = 5(cm)

= [ ; = [10; 901 2 ]T ]T

= [ ; = [0; 0.5 (m/s)0X x y]T ]T

1(seg) 0.001(seg)

= [ ;1 2 ]T

(rad/seg) = [ ; ] 1 2T )(0H X = [x; y; ]

T

(Nm) T = [ ;1 2 ]T

g Y

In [170]: #parametros

L_1 = 1L_2 = .5#masasm_1 = 0.05*0.05*L_1*7806m_2 = 0.05*0.05*L_2*7806Lc1 = L_1/2Lc2 = L_2/2# Constantes de los tensores de inercia:Izz1 = 1.6303Izz2 = 0.2053;# Gravedad:g = 9.81

Creamos un vector de tiempos y las constantes necesarias

In [7]: # paso de tiempodt = 0.01# tiempo de simulaciont_sim = 1.#Cantidad de iteracionesN = t_sim/dt#vector de tiempot = np.arange(N)*dt;

Creamos los vectores de coordenadas generalizadas y seteamos las condiciones iniciales

In [25]: # Aceleraciones generalizadasddQ = np.zeros((2,N-1))# Velocidades generalizadasdQ = np.zeros((2,N))# Coordenadas generalizadasQ = np.zeros((2,N))Q_0 = np.array([10.,90.])*(np.pi/180)Q[:,0] = Q_0

Creamos los vectores de posicion, de velocidad, torque, las matrices para computar los coeficientes de Christoffel

y los terminos de la ecuacin de Lagrange

In [29]: # velocidades cartesianasdX = np.array([0., 0.5])# coordenadas cartesianasX = np.zeros((3,N)) #[x,y,phi]# determinantes del JacobianoD = np.zeros((1,N))

# momentos de torsion activosT = np.zeros((2,N-1))Tsg = np.zeros((2,N-1))

# vector de gravedadG = np.zeros((2,N))# coeficientes de ChristoffelC = np.zeros((2,2,2,N))c112 = np.zeros((1,N))# matriz de masasM = np.zeros((2,2,N))

# Terminos de la ecuacion de LagrangeTerm1 = np.zeros((2,N-1))Term2 = np.zeros((2,N-1))

In [145]: #calculo de las trayectoriasfor n in xrange(int(N)): # constantes para simplificar la expresion de J: c1 = np.cos(Q[0,n]) s1 = np.sin(Q[0,n]) c2 = np.cos(Q[1,n]) s2 = np.sin(Q[1,n]) c12 = np.cos(Q[0,n]+Q[1,n]) s12 = np.sin(Q[0,n]+Q[1,n]) # Matriz Jacobiana J = np.array([[-L_1*s1-L_2*s12,-L_2*s12],[L_1*c1+L_2*c12, L_2*c12]]) # Velocidades generalizadas dQ[:,n] = np.linalg.solve(J,dX) # coordenadas generalizadas if(n != N): Q[:,n] = Q[:,n]+dQ[:,n]*dt # aceleraciones generalizadas if(n!=1): ddQ[:,n-1]=(dQ[:,n]-dQ[:,n-1])/dt # coordenadas cartesianas X[0,n] = L_1*c1+L_2*c12 X[1,n] = L_1*s1+L_2*s12 X[2,n] = Q[0,n]+Q[1,n] # determinante del Jacobiano D[0,n] = np.linalg.det(J) # matriz de masas M11 = m_2*(Lc2**2+L_1**2+2*c2*L_1*Lc2)+m_1*Lc1**2+Izz1+Izz2 M12 = m_2*(Lc2**2+L_1*c2*Lc2)+Izz2 M22 = m_2*Lc2**2+Izz2 M[:,:,n] = np.array([[M11,M12],[M12,M22]]) # simbolos de Christoffel c112 = L_1*Lc2*m_2*s2; C[:,:,0,n] = np.array([ [ 0., -c112],[ -c112, -c112]]) C[:,:,1,n] = np.array([ [c112, 0],[ 0, 0]]) G[:,n] = g* np.array([m_2*(Lc2*c12+L_1*c1)+m_1*Lc1*c1, m_2*Lc2*c12]) # momentos de torsion activos if(n != 1): k = n-1 # terminos de la ecuacion de Lagrange Term1[0,k] = np.dot(M[0,:,k] , ddQ[:,k]) Term2[0,k] = dQ[:,k].dot(C[:,:,1,k]).dot( dQ[:,k]) Term1[1,k] = np.dot(M[1,:,k] ,ddQ[:,k]) Term2[1,k] = dQ[:,k].dot(C[:,:,1,k]).dot(dQ[:,k]) # sin gravedad Tsg[0,k] = Term1[0,k] + Term2[0,k] Tsg[1,k] = Term1[1,k] + Term2[1,k] # con gravedad: T[0,k] = Tsg[0,k] + G[0,k] T[1,k] = Tsg[1,k] + G[1,k]

In [146]: %pylab inline#ajustamos el tamao de la imagenplt.rcParams['figure.figsize'] = 10,8

plt.plot(t,Q[0,:]*180/np.pi,t,Q[1,:]*180/np.pi,'k-')plt.title('Coordenadas generalizadas', fontsize=23)plt.xlabel(r'$t$', fontsize=20)plt.ylabel(r'$Q$', fontsize=20)plt.legend([r'$\theta_{1}$',r'$\theta_{2}$'],fontsize=20)plt.show()

Populating the interactive namespace from numpy and matplotlib

In [147]: plt.plot(t,dQ[0,:]*180/pi,t,dQ[1,:]*180/np.pi,'r--')plt.title('Velocidades generalizadas ', fontsize=23)plt.xlabel(r'$t$', fontsize=20)plt.ylabel(r'$\dot{Q}$', fontsize=20)plt.legend([r'$\dot{\theta}_{1}$',r'$\dot{\theta}_{2}$'],2,fontsize=20)plt.show()

In [148]: plt.plot(t[0:-1],ddQ[0,:]*180/np.pi,t[0:-1],ddQ[1,:]*180/np.pi,'--')plt.title('Aceleraciones generalizadas',fontsize=23)plt.xlabel(r'$t$', fontsize=20)plt.ylabel(r'$\ddot{Q}$', fontsize=20)plt.legend([r'$\ddot{\theta}_{1}$',r'$\ddot{\theta}_{2}$'],2,fontsize=20)plt.show()

In [149]: plt.plot(t,X[0,:],t,X[1,:],'--',t,X[2,:],'.-')plt.title('Coordenadas cartesianas',fontsize=23)plt.xlabel(r'$t$', fontsize=20)plt.ylabel(r'$X$', fontsize=20)plt.legend([r'$x$',r'$y$',r'$\phi$'],fontsize=20)

Out[149]:

In [157]: plt.plot(t[0:-1],T[0,:],t[0:-1],T[1,:],'k--')plt.title('Momentos de torsion',fontsize=23)plt.xlabel(r'$t$', fontsize=20)plt.ylabel('Momentos', fontsize=20)plt.legend([r'$\tau_{1}$',r'$\tau_{2}$'],2,fontsize=20)plt.show()

In [160]: plt.plot(t[0:-1],Tsg[0,:],t[0:-1],Tsg[1,:],'--')plt.title('Momentos de Torsion activos sin gravedad',fontsize=23)plt.xlabel(r'$t$', fontsize=20)plt.ylabel('Momentos', fontsize=20)plt.legend([r'$\tau_{1}$',r'$\tau_{2}$'],2,fontsize=20)plt.show()

In [164]: plot(t[0:-1],Term1[0,:],t[0:-1],Term2[0,:],'--',t,G[0,:],'-.')plt.title('Terminos de la ecuacion de Lagrange',fontsize=23)plt.xlabel(r'$t$', fontsize=20)plt.ylabel('Torque', fontsize=20)plt.legend([r'$\mathbf{M(\Theta)\ddot{\Theta}}$',r'$\mathbf{V(\Theta,\dot{\Theta})}$',r'$\mathbf{G(\Theta)}$'],3,fontsize=20)

Out[164]:

Back to top

In [168]: plt.plot(t[0:-1],Term1[1,:],t[0:-1],Term2[1,:],'--',t,G[1,:],'-.')plt.title('Terminos de la ecuacion de Lagrange(segundo item)',fontsize=23)plt.xlabel(r'$t$', fontsize=20)plt.ylabel('Torque', fontsize=20)plt.legend([r'$\mathbf{M(\Theta)\ddot{\Theta}}$',r'$\mathbf{V(\Theta,\dot{\Theta})}$',r'$\mathbf{G(\Theta)}$'],3,fontsize=20)

Podemos observar como al agregar el trmino de gravedad en se refleja en el vector con respecto a la

simulacin que no requera esto.

In []:

More info on IPython website (http://ipython.org). The code for this site (https://github.com/ipython

/nbviewer) is licensed under BSD (https://github.com/ipython/nbviewer/blob/master/LICENSE.txt). Some icons from

Glyphicons Free (http://glyphicons.com), built thanks to Twitter Bootstrap (http://twitter.github.com/bootstrap/)

This web site does not host notebooks, it only renders notebooks available on other websites. Thanks to all our

contributors (https://github.com/ipython/nbviewer/contributors).

nbviewer version: cf5a2ed (https://github.com/ipython/nbviewer/commit

/cf5a2edf0cff4cc8b7d06c525a0a4294e64921bf) (Tue, 17 Dec 2013 19:10:24 -0800)

Out[168]:

Y G()