Dinamica L-E

Dinámica Euler-Lagrange

Crear Matriz de parámetros de Denavit-Hartenberg

Crear matriz del vector masa (M)

Crear Matriz de Coordenadas Generalizadas de los eslabones

Declarar variable n igual al número de grados de libertad

Bucle para que i tome valores del 1 hasta n

Desarrollar Matriz homogéneas (A(i-1)(i))

Declarar variable T con valor de una matriz identidad de 4x4

Guardar en la variable T el resultado de la operación T x A(i-1)(i)

Guardar el valor de T en una variable A(0)(i)

1

1

Declarar variable T con valor de una matriz identidad de 4x4

Bucle para que i tome valores del 1 hasta n

Declarar variable A(i-1)(i-1) con valor de una matriz identidad de 4x4

Declarar variable Q(i) con los valores para definir un eslabón rotacional

Guardar en la variable T el resultado de la operación T x A(i-1)(i)

Guardar el valor de T en una variable A(i-1)(i)

2

2

Bucle para que i tome valores del 1 hasta n

Bucle para que i tome valores del j hasta n

¿j es igual o menor a i?

Se intenta realizar la operación para obtener U(i)(j)

Si

U(i)(j)=0

¿Se pudo realizar la operación?

Si

No

U(i)(j)=0

No

3

3

Bucle para que i tome valores del 1 hasta n

4

Bucle para que j tome valores del 1 hasta n

Bucle para que k tome valores del 1 hasta n

¿i es igual o mayor a k? y ¿k es igual o mayor a j?

Se intenta realizar la operación para obtener U(i)(j)(k)

¿Se pudo realizar la operación?

¿i es igual o mayor a j? y ¿j es igual o mayor a k?

Si

Si

U(i)(j)(k)=0

No

No

Se intenta realizar la operación para obtener U(i)(j)(k)

¿Se pudo realizar la operación?

U(i)(j)(k)=0

No

Si

No

Si

4

Se crean variables L(i) que representan las longitudes del centro de masa de cada eslabón

Se crean matrices J(i) que representan los tensores de inercia de su respectivo eslabón

Bucle para que j tome valores del 1 hasta n

Bucle para que j tome valores del 1 hasta n

Se declara una variables DD=0

Bucle para que k tome los valores máximos de i y j hasta n

Se intenta realizar la operación DD=DD+U(i)(j)*J(k)*traspuesta de U(i)(k)

¿Se pudo realizar la operación?

DD=0

No

Guardamos el resultado de la operación dentro de la matriz simétrica de aceleración con el nombre D

Si

Se muestra la Matriz final D

5

Bucle para que i tome valores del 1 hasta n

6

Bucle para que K tome valores del 1 hasta n

Bucle para que m tome valores del 1 hasta n

5

Se declara una variable hh=0

Bucle para que j tome los valores máximos de i, k y m hasta n

Se intenta realizar la operación hh=hh+U(j)(k)(m)*J(j)*traspuesta de U(i)(j)

¿Se pudo realizar la operación?

hh=0

No

Si

6

Crear Matriz de las coordenadas generalizadas de la velocidad (qv)

Bucle para que i tome valores del 1 hasta n

Se declara una variable kk=0

Bucle para que k tome valores del 1 hasta n

Se declara una variables mm=0

Bucle para que m tome valores del 1 hasta n

Se intenta realizar la operación mm=mm+h(i)(k)(m)*qv(k)*qv(m)

¿Se pudo realizar la operación?

mm=0

No

Se realiza la operación kk=kk+mm

Guardamos el resultado de la operación dentro de la matriz que representa el vector de las Fuerzas de Coriolis (H)

7

Se muestra la Matriz final H

7

Crear Matriz para representar el valor de la gravedad

Crear Matrices que representen las longitudes de los centros de masa en el eje en el que se encuentra

Bucle para que i tome valores del 1 hasta n

Se declara una variable cc=0

Bucle para que j tome valores del 1 hasta n

Se intenta realizar la operación cc=cc-M(j)*G*U(j)(i)*r(j)

¿Se pudo realizar la operación?

cc=0

Guardamos el resultado de la operación dentro de la matriz que representa las Fuerzas de Gravedad (C)

8

Se muestra la Matriz final C

8

Crear Matriz generalizada de aceleración (qoo)

Realizar la operación Q=D*qoo+H+C la cual es la ecuación de la Dinámica Euler-Lagrange

Mostrar los torques (T(i)) para los motores de cada eslabón

FIN

Bucle para que i tome valores del 1 hasta n

Desarrollar Matriz homogénea n

Multiplicar la Matriz Homogénea por las anteriores a ella

Guardar el en una variable A(0)(i)

Cinemática Directa

Crear Matriz de parámetros de Denavit-Hartenberg

FIN

Hasta que la resta del valor deseado menos el valor obtenido sea menor al valor pequeño propuesto

Desarrollar Matriz homogénea n

Multiplicar la Matriz Homogénea por las anteriores a ella

Cinemática Inversa

Ingresar los valores actuales de q de cada eslabón

Graficar el resultado con cinemtica directa

FIN