E.P. 1.

4 - NE - Antropomorfico

LUIS ANTONIO PADILLA CRUZ - 2018030056

JOSE LUIS SANCHEZ GONZALEZ - 2018030872
LUIS FERNANDO FLORES SANDOVAL - 2019030022

Docente:
M.F. RAFAEL ANGEL NARCIO LAVEAGA
clear
close all
clc

1.-Datos del robot

%1.1.- Definimos el tiempo como una variable simbólica
syms t real positive;

1
 %1.2.- Definimos las masas de las articulaciones como variables simbólicas
syms m_1 m_2 m_3 real positive;
%1.3.- Definimos las coordenadas generalizadas como funciones simbólicas
syms q_1(t) q_2(t) q_3(t);
%1.4.- Armamos un vector que contenga la información de la configuración
%del robot, colocando un 1 para representar una junta prismática, y un 0
%para representar una junta de revoluta
RP=[0;0;0];
%1.5.- A fin de después poder utilizar un ciclo for en el cálculo de las
%fuerzas, debemos tener un método para indexar las masas, por lo que
%declaramos un vector para contener las masas de las articulaciones
m = formula([m_1;m_2;m_3]);
%1.6.- Armamos el vector de coordenadas generalizadas
q = formula([q_1;q_2;q_3]);
%1.7.- Obtenemos el vector de velocidades generalizadas al derivar respecto
%al tiempo al vector de coordenadas generalizadas
qp = formula(diff(q,t));
%1.8.- Revisamos si el número de masas de las articulaciones que se ha
%declarado es igual al número de elementos del vector de coordenadas
%generalizadas, y si el número de elementos de ambos es igual a la cantidad
%de elementos del vector de la configuración del robot. Si todos estos
%vectores tienen la misma cantidad de elementos, definimos el número de
%grados de libertad del robot, en caso contrario, el codigo marcará error.
if and(and(size(q,1)==size(RP,1),size(q,1)==size(m,1)),size(RP,1)==size(m,1))
GDL = size(q,1);
disp("Número de grados de grados de libertad del robot: ")
disp(GDL)
else
error("Número de grados de libertad no congruente")
end

Número de grados de grados de libertad del robot:

3

Después, definimos cuál es el eje vertical en el marco de referencia inercial, mismo que será el eje sobre
el cuál actué la gravedad, a la cuál colocaremos en dirección opuesta con respecto hacia dónde actua (por
ejemplo, si la gravedad está en eje Yy actuá hacia el negativo, la pondremos con signo positivo en el eje Y.
Quedando de la siguiente forma según el caso:

Para este caso en particular, definimos el eje vertical como el eje Y, siendo la dirección hacia arriba y
hacia abajo, por lo que la gravedad en este caso actúa sobre , quedando por lo tanto el vector de
aceleración de gravedad en este caso como:

%1.9- Declaramos la aceleración de gravedad como variable simbólica

2
 syms g real;
%1.10.- Definimos el vector de aceleración de la gravedad
G = [0;g;0];

Después de esto, procedemos a obtener las matrices de transformación locales correspondientes al robot a
partir de su tabla de parámetros de Denavit - Hartenberg (DH), para esto utilizamos la función que habíamos
desarrollado previamente para este próposito:
function [A] = DH_kine(theta, d, a, alpha)
%A partir de los parámetros de DH para una articulación, construye
%su matriz local "A".
%1.- Declaramos las matrices de rotacion que usaremos para crear las
%transformadas homogéneas dependientes de un solo parámetros que se han de
%multiplicar para la obteción de la local
R_t = Rot_z(sym(theta));
R_0 = eye(3);
R_a = Rot_x(sym(alpha));
%2.- De la misma forma que declaramos las matrices de rotación a utilizar,
%ahora declaramos los vectores de posición que usaremos.
P0 = zeros(3,1);
Pd = [0;0;sym(d)];
Pa = [sym(a);0;0];
%3.- Una vez teniendo tanto las matrices de rotación con los vectores de
%posición, procedemos a armas las transformadas homogéneas dependientes de
%un solo parámetro:
T_t = TH(R_t, P0);
Td = TH(R_0, Pd);
Ta = TH(R_0, Pa);
T_a = TH(R_a,P0);
%4.- Finalmente, procedemos a multiplicar las transformadas homogéneas
%dependientes de una sola variable.
A = simplify(T_t*Td*Ta*T_a);
end

La cuál hace uso de otras 3 funciones que habíamos desarrollado:

1.- Para obtener una matriz de rotación en el eje X ( ) a partir de un ángulo :

function [Rx] = Rot_x(alpha)
%Rot_x La función nos sirve para declarar una matriz de rotación en el eje
%x a partir del ángulo alpha.
Rx = [1 0 0;
0 cos(alpha) -sin(alpha);
0 sin(alpha) cos(alpha)];
end

2.- Para obtener una matriz de rotación en el eje Z ( ) a partir de un ángulo .

function [Rz] = Rot_z(theta)
%Rot_z La función nos sirve para declarar una matriz de rotación en el eje
%z a partir del ángulo theta.
Rz = [cos(theta) -sin(theta) 0;
sin(theta) cos(theta) 0;
0 0 1];
end

3
3.- Para armar una matriz de transformación, o transformada homógenea a partir de una matriz de rotación y
un vector de posición.
function [T] = TH(R, P)
vec0 = zeros(1,3);
T = [R P; vec0 1];
end

A modo de ejemplo, podemos sustituir los elementos directamente en la matriz de una local para DH en su
forma general:

syms d_i a_i alpha_i theta_i real;

disp("Matriz de Denavit Hartenberg")

Matriz de Denavit Hartenberg

DH_M = DH_kine(theta_i,d_i,a_i,alpha_i);
disp(DH_M)

Una vez haciendo esto, para obtener las matrices locales, ya solo es cuestión de ingresar los valores
apropiados para el robot del cuál queramos obtener el modelo dinámico. Quedando para este caso:

%1.11.- Declaramos como variables simbólicas las longitudes constantes de las articulaciones.
syms l_1 l_2 l_3 real positive;
%Como este robot no tiene longitudes constantes, nos saltamos este paso.
%1.12.- Obtenemos las matrices de transformación locales a partir de la
%tabla de parámetros de DH
A(:,:,1) = DH_kine(q_1,l_1,0,pi/2);
disp("A1=")

A1=

disp(A(:,:,1))

A(:,:,2) = DH_kine(q_2,0,l_2,0);
disp("A2=")

A2=

disp(A(:,:,2))

4
 A(:,:,3) = DH_kine(q_3+pi/2,0,l_3,pi/2);
disp("A3=")

A3=

disp(A(:,:,3))

Una vez teniendo las matrices locales, volvemos a proceder a verificar que no haya inconsistencias en cuánto
al número de grados de libertad del robot, ya que si se declara un número de matrices locales diferente al
número de grados de libertad del robot el cálculo sería incorrecto, incompleto, o marcaría error.

%1.13.- Verificamos que no haya inconsistencias en cuánto al número de

%matrices locales con el número de grados de libertad
if size(A,3)~=GDL
error("No coincide el número de grados de libertad con el número de matrices locales")
end

Y repetimos para las locales en los centros de masa

%1.14.- Declaramos como variables simbólicas las longitudes constantes de

%las articulaciones.
syms lc_1 lc_2 lc_3 real positive;
%Como este robot no tiene longitudes constantes, nos saltamos este paso.
%1.15.- Obtenemos las matrices de transformación locales a partir de la
%tabla de parámetros de DH
Ac(:,:,1) = DH_kine(q_1,0,lc_1,0);
disp("Ac_1=")

Ac_1=

disp(Ac(:,:,1))

Ac(:,:,2) = DH_kine(q_2,0,lc_2,0);
disp("Ac_2=")

5
 Ac_2=

disp(Ac(:,:,2))

Ac(:,:,3) = DH_kine(q_3,0,lc_3,0);
disp("Ac_3=")

Ac_3=

disp(Ac(:,:,3))

%1.16.- Verificamos que no haya inconsistencias en cuánto al número de

%matrices locales con el número de grados de libertad
if size(Ac,3)~=GDL
error("No coincide el número de grados de libertad con el número de matrices locales")
end

Una vez hecho esto, dado que concideraremos cada articulación del robot como un cuerpo rígido, debemos de
definir el tensor de inercia visto desde el centro de masas de cada articulación. Como esto generalmente nos lo
da el fabricante, utilizamos en este caso tensores de inercia genéricos:

%1.17.- Declaramos los datos que necesitaremos para los tensores de inercia
%genéricos vistos desde el centro de masa de la articulación
syms Ixx_1 Iyy_1 Izz_1 Ixx_2 Iyy_2 Izz_2 Ixx_3 Iyy_3 Izz_3 real positive;
%1.18.- Declaramos los tensores de inercia genéricos vistos desde el centro de masa de la artic
I_c(:,:,1) = formula([Ixx_1,0,0;0,Iyy_1,0;0,0,Izz_1]);
I_c(:,:,2) = formula([Ixx_2,0,0;0,Iyy_2,0;0,0,Izz_2]);
I_c(:,:,3) = formula([Ixx_3,0,0;0,Iyy_3,0;0,0,Izz_3]);
%1.19.- Verificamos que no haya inconsistencias en cuánto al número de
%tensores de inercia con respecto al número de articulaciones
if size(I_c,3)~=GDL
error("No coincide el número de articulaciones con el número de tensores de inercia")
end

2.- Cinemática directa

%2.1.- Declaramos el vector Z0, que tiene el mismo valor para todos los
%robots que tienen un marco de referencia inercial fijo
Z0 = [0;0;1];

6
%2.2.- Iniciamos el ciclo for que utilizaremos para calcular la cinemática
%directa del robot y extraer los elementos que nos interesan
for i=0:(GDL-1)
i_str = num2str(i);
i1_str = num2str(i+1);
%2.3.- Calculamos las globales
try
H(:,:,i+1)= simplify(H(:,:,i)*A(:,:,i+1));
Hc(:,:,i+1)= simplify(H(:,:,i)*Ac(:,:,i+1));
catch
H(:,:,i+1)= A(:,:,i+1); %Para A1, ya que no se declaró H0
Hc(:,:,i+1)= Ac(:,:,i+1);
end
disp(strcat("H^",i_str,"_",i1_str))
disp(H(:,:,i+1))
disp(strcat("Hc^",i_str,"_",i1_str))
disp(Hc(:,:,i+1))
%2.4.- Extraemos los vectores Z vistos desde el marco de referencia
%inercial
try
Z(:,i+1) = Hc(:,3,i)
catch
Z(:,i+1) = Z0;
end
disp(strcat("Z^0_",i_str))
disp(Z(:,i+1))
%2.5.- Jacobiano angular
if (RP(i+1)==0)
%Si la junta es de revoluta
Jw(:,i+1)= Z(:,i+1);
else
%Si la junta es prismática
Jw(:,i+1)= [0;0;0];
end
%2.6.- Obtenemos los vectores de posición de los centros de masa vistos
%desde el marco de referencia inercial
disp(strcat("Oc_",i1_str))
Oc(:,i+1)=simplify(Hc(1:3,4,i+1));
disp(Oc(:,i+1))
end

H^0_1

Hc^0_1

7
Z^0_0
0
0
1
Oc_1

H^1_2

Hc^1_2

Z^0_1
0
0
1
Oc_2

H^2_3

Hc^2_3

8
 Z^0_2
0
0
1
Oc_3

disp("Jw")

Jw

disp(Jw)

0 0 0
0 0 0
1 1 1

3.- Obtenemos las velocidades y las energías

for i=1:GDL
i_str=num2str(i);
%3.1.- Calculamos las velocidades angulares
disp(strcat("w_",i_str))
w(:,i) = Jw(:,1:i)*qp(1:i);
disp(w(:,i))
%3.2.- Calcular las velocidades lineales
disp(strcat("V_",i_str))
V(:,i) = diff(Oc(:,i),t);
disp(V(:,i))
%3.3.- Calculamos la componente lineal de la energía cinética
disp(strcat("Tl_",i_str))
Tl(i) = simplify((1/2)*V(:,i).'*m(i)*V(:,i));
disp(Tl(i))
%3.4.- Calculamos la componente angular de la energía cinética
disp(strcat("Tw_",i_str))
Tw(i) = simplify((1/2)*w(:,i).'*I_c(:,:,i)*w(:,i));
disp(Tw(i))
%3.5.- Energía cinética de la articulación
T(i) = Tl(i)+Tw(i);

9
 %3.6.- Energía potencial
disp(strcat("P_",i_str))
P(i) = m(i)*G.'*Oc(:,i);
disp(P(i))
end

w_1

V_1

Tl_1

Tw_1

P_1

w_2

V_2

Tl_2

Tw_2

10
P_2

w_3

V_3

Tl_3

Tw_3

P_3

%3.7.- Calculamos la energía cinética total

T_t = sum(T);
%3.8.- Calculamos la energía potencial total

11
 P_t = sum(P);
%3.9.- Obtenemos la energía total del robot
E = T_t+P_t;
%3.10.- Obtenemos el Lagrangiano
L = T_t-P_t;

4.- Ecuaciones de movimiento

Una vez calculando el lagrangiano, procedemos a resolver la ecuación de Euler - Lagrange para cada
articulación

for i=1:GDL
i_str=num2str(i);
disp(strcat("Tau_",i_str))
Tau(i) = -functionalDerivative(L,q(i));
disp(Tau(i))
end

Tau_1

Tau_2

12
Tau_3

13