Dadas :
$L_{1}= 1.0(m)$, $L_{2}=0.5(m)$ ambos vínculos son de acero sólido con una densidad de masa $\rho=7806(kg/m^{3})$ ambos tienen las dimensiones de anchura y grosor $\omega=t=5(cm)$. Se supone que las articulaciones angulares son perfectas, conectando los vínculos precisamente en sus extremos(lo que no es fisicamente posible )
Los ángulos iniciales son :
$\Theta=[\theta_{1};\theta_{2}]^{T}=[10;90]^{T}$
La velocidad cartesiana comandada(constante) es : $^{0}\dot{X}=[\dot{x} ; \dot{y}]^{T}=[0 ; 0.5]^{T}(m/s)$
Simule el movimiento en $1(seg)$, con un intervalo de tiempo de control de $0.001(seg)$. Presente cinco trazos(cada conjunto en un gráfico separado ) :
Los dos ángulos de articulación (grados) $\Theta=[\theta_{1};\theta_{2}]^{T}$ contra el tiempo
Las dos velociadades de articulación $(rad/seg)$ $\dot{\Theta}=[\dot{\theta}_{1};\dot{\theta}_{2}]$
Las dos aceleraciones de articulación de $(^{0}_{H}T)$, $X=[x;y;\phi]^{T}$ (rad)
Los dos momentos de torsión de articulación de dinámica inversa $(Nm)$ $T=[\tau_{1};\tau_{2}]^{T}$ contra el tiempo
Realice la simulación dos veces. La primera ignore la gravedad(el plano de movimiento es normal al efecto de la gravedad ); la segunda vez considere la gravedad $\vec{g}$ en la dirección negativa de $Y$
Comenzamos la resolución planteando y asignando todas las constantes y datos del problema:
In [169]:
import numpy as np
In [170]:
#parametros
L_1 = 1
L_2 = .5
#masas
m_1 = 0.05*0.05*L_1*7806
m_2 = 0.05*0.05*L_2*7806
Lc1 = L_1/2
Lc2 = L_2/2
# Constantes de los tensores de inercia:
Izz1 = 1.6303
Izz2 = 0.2053;
# Gravedad:
g = 9.81
Creamos un vector de tiempos y las constantes necesarias
In [7]:
# paso de tiempo
dt = 0.01
# tiempo de simulacion
t_sim = 1.
#Cantidad de iteraciones
N = t_sim/dt
#vector de tiempo
t = np.arange(N)*dt;
Creamos los vectores de coordenadas generalizadas y seteamos las condiciones iniciales
In [25]:
# Aceleraciones generalizadas
ddQ = np.zeros((2,N-1))
# Velocidades generalizadas
dQ = np.zeros((2,N))
# Coordenadas generalizadas
Q = 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 ecuación de Lagrange
$\mathbf{M}(\Theta)\ddot{\Theta}+\mathbf{V}(\Theta,\dot{\Theta})+\mathbf{G}(\Theta)=\tau$
y los simbolos de Christoffel se definen:
$c_{ijk}:=\frac{1}{2}\{ \frac{\partial d_{kj}}{\partial q_{i}} +\frac{\partial d_{ki}}{\partial q_{j}} +\frac{\partial d_{ij}}{\partial q_{k}} \}$
In [29]:
# velocidades cartesianas
dX = np.array([0., 0.5])
# coordenadas cartesianas
X = np.zeros((3,N)) #[x,y,phi]
# determinantes del Jacobiano
D = np.zeros((1,N))
# momentos de torsion activos
T = np.zeros((2,N-1))
Tsg = np.zeros((2,N-1))
# vector de gravedad
G = np.zeros((2,N))
# coeficientes de Christoffel
C = np.zeros((2,2,2,N))
c112 = np.zeros((1,N))
# matriz de masas
M = np.zeros((2,2,N))
# Terminos de la ecuacion de Lagrange
Term1 = np.zeros((2,N-1))
Term2 = np.zeros((2,N-1))
In [145]:
#calculo de las trayectorias
for 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 tamaño de la imagen
plt.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()
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]:
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)
Out[168]:
Podemos observar como al agregar el término de gravedad en $Y$ se refleja en el vector $\mathbf{G}(\Theta)$ con respecto a la simulación que no requería esto.
In [ ]: