E.P. 1.

4 - NE - Robot esferico

LUIS ANTONIO PADILLA CRUZ - 2018030056

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

Docente:
M.F. RAFAEL ANGEL NARCIO LAVEAGA

1
 clear
close all
clc

Robot esferico
1.-Datos del robot
En este ejemplo obtendremos las ecuaciones de movimiento para el robot cartesiano utilizando el método
recursivo de Newton - Euler.

Para ello el primer paso es saber cuántos grados de libertad tiene el robot, para construir el vector de
coordenadas generalizadas y, subsecuentemente, el vector de velocidades generalizadas y de aceleraciones
generalizadas; además de saber la configuración del robot, es decir, saber que tipo de junta es cada una;
debemos de conocer la masa de cada articulación.

%1.1.- Definimos el tiempo como una variable simbólica

syms t real positive;
%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;1];
%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.- Derivamos el vector de velocidades generalizadas para obtener las
%aceleraciones generalizadas
qpp = formula(diff(qp,t));
%1.9.- 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

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

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

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

Después de esto, obtenemos las matrices de transformación locales para este robot a partir de su
tabla de parámetros de Denavit Hartenberg (DH).

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

3
 %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.- 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.12.- Declaramos como variables simbólicas las longitudes constantes de

%las articulaciones.

4
 syms l_1 l_2 real positive;
%Como este robot no tiene longitudes constantes, nos saltamos este paso.
%1.13.- 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+pi/2,0,0,pi/2);
disp("A2=")

A2=

disp(A(:,:,2))

A(:,:,3) = DH_kine(0,q_3,0,0);
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

5
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.14.- 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.15.- 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.16.- 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

Y finalmente, escribimos las distancias entre los centros de masa de cada articulación, y el inicio de la
articulación.

%1.17.- Definimos las distancias constantes

syms lc_1 lc_2 lc_3;
Pc(:,1)=[lc_1;0;0];
Pc(:,2)=[lc_2;0;0];
Pc(:,3)=[lc_3;0;0];

2.- Cinemática directa

El siguiente paso es calcular la cinemática directa del robot, esto es necesario ya que debemos de calcular
las globales y extraer algunos datos previo a utilizar la formulación de Newton - Euler. Pero dado que también
utilizaremos algunos elementos de las matrices de rotación locales , aprovecharemos para extraer
también estos elementos. Siendo estos elementos a extraer especificamente los vectores de posición ,y
las matrices de rotación y a partir de estas calcular las matrices de rotación . Para esto recordemos
que toda matriz de transformación está compuesta de la siguiente manera:

dónde es la matriz de transformación de i vista desde j; la matriz de rotación de i vista

desde j; y el vector de posición de i visto desde j; por lo tanto, las matrices transformación locales están
compuestas de la misma forma:

= por lo tanto podemos extraer ambos datos a partir de las matrices de transformación

locales .

Y para calcular simplemente calculamos la traspuesta de la matriz de rotación , dado que:

6
También debemos de ajustar los vectores de posición de forma tal que sean vistos desde la misma
articulación:

Mientras que de las globales (dónde ) nos interesan

solamente sus matrices de rotación y , para a partir de estás obtener los vectores que no

son otra cosa más que la tercera columna de la matriz de rotación de la global . Y después cambiamos la
orientación de los vectores Z de forma tal que sean vistos desde el marco de referencia i:

O bien, como matlab no permite que los indices comiencen en 0, utilizaremos:

en el código.

Usar i en vez de en los índices de los vectores Z fue a próposito, ya que requerimos conocer también
el vector ; mismo que se obtiene a partir de la matriz global , que es igual para todos los robots, y
representa la matriz de transformación del marco de referencia inercical respecto a si mismo, pero dado que
es inercial (no presenta movimiento, ni de rotación ni de traslación) está matriz toma la forma de una matriz
identidad de 4x4 :

Por lo que su matriz de rotación siempre se trata de una matriz identidad de 3x3 .

Dado que ambas matrices son de identidad, no producen cambios al multiplicarlas por un vector en cuanto
a magnitud, dado que sería equivalente a multiplicar por 1. Por lo tanto el vector tiene el mismo
valor para todos los robots:

7
Por lo que lo podemos declarar aparte para mantener el mismo indexado que en las ecuaciones que
utilizaremos para esta formulación. Vale volver a mencionar que en el código recorreremos los índices para los
vectores Z.

%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];
%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.- Extraemos las matrices de rotación locales y obtenemos su
%traspuesta
disp(strcat("R^",i_str,"_",i1_str))
R(:,:,i+1) = A(1:3,1:3,i+1);
disp(R(:,:,i+1))
disp(strcat("R^",i1_str,"_",i_str))
R_T(:,:,i+1)= R(:,:,i+1).';
disp(R_T(:,:,i+1))
%2.4.- Extraemos los vectores de posición locales
P(:,i+1) = A(1:3,4,i+1);
disp(strcat("P^",i_str,"_",i1_str))
disp(P(:,i+1))
%2.5.- Colocamos los vectores de posición locales de tal manera que
%sean vistos desde el centro de masa de su respectiva articulación
P_i(:,i+1) = R_T(:,:,i+1)*P(:,i+1);
disp(strcat("P^",i1_str,"_",i1_str))
disp(P_i(:,i+1))
%2.6.- Calculamos las globales
try
H(:,:,i+1)= simplify(H(:,:,i)*A(:,:,i+1));
catch
H(:,:,i+1)= A(:,:,i+1); %Para A1, ya que no se declaró H0
end
disp(strcat("H^",i_str,"_",i1_str))
disp(H(:,:,i+1))
%2.7.- Extraemos las matrices de rotación de las globales y obtenemos
%su traspuesta
R0(:,:,i+1) = H(1:3,1:3,i+1);
R0_T(:,:,i+1)= R0(:,:,i+1).';
%2.8.- Calculamos los vectores Z vistos desde i+1, desplazando su
%índice un poco para facilitar el código (Z1=Z00, Z2=Z01, etc)
disp(strcat("Z^",i_str,"_",i_str))
try
Z_h(:,i+1) = R0_T(:,:,i+1)*R0(1:3,3,i);
catch
Z_h(:,i+1) = R0_T(:,:,i+1)*Z0; %Para incluir a Z0
end
disp(Z_h(:,i+1))
end

R^0_1

8
R^1_0

P^0_1

P^1_1

H^0_1

Z^0_0

R^1_2

R^2_1

P^1_2

P^2_2

H^1_2

9
Z^1_1

R^2_3

R^3_2

P^2_3

P^3_3

H^2_3

Z^2_2

10
3.- Iteraciones salientes
Una vez calculando la cinemática directa del robot, y obteniendo los datos que necesitamos, podemos empezar
a utilizar la formulación iterativa de Newton - Euler, misma que se divide en 2 partes:

Iteraciones salientes - En estás se calcula la propagración de la velocidad angular, y de las aceleraciones.

Además de usar la segunda ley de Newton para obtener la fuerza causada por las

aceleraciones , y la ecuación de Euler para calcular los momentos de torsión o

torque causada por la velocidad angular y la aceleración angular (dónde es el tensor de inercia
visto desde el centro de masas, y alineado con los ejes principales de inercia). Para esta parte utilizaremos un
ciclo for que vaya desde 0 hasta dónde n es el número de grados de libertad del robot.

Iteraciones entrantes - En estás se calculan la propagación de la fuerza y el momento de torsión entre las

articulaciones, obteniendo la fuerza neta y el momento de torsión o torque neto para cada articulación,

encontrando así el vector de fuerzas generalizadas , conteniendo este las ecuaciones de movimiento para
todas las juntas del robot. Para esta parte utilizaremos un ciclo for que vaya desde n hasta 1 dónde n es el
número de grados de libertad del robot; como Matlab no permite tener un ciclo for que vaya en este orden,
utilizaremos una variable (dónde i irá de 0 a ) para las iteraciones.

En este caso, como en el de todos los robots manipuladores que se encuentren en una base fija, supondremos

que su velocidad angular en el marco de referencia inercial es 0 , al igual que su aceleración

angular en el marco de referencia inercial , y la única aceleración lineal a la que está sometida el

marco de referencia inercial es la aceleración de gravedad .

%3.1.- Iniciamos el ciclo for para las iteraciones salientes

for i=0:(GDL-1)
i_str = num2str(i+1);
%3.2.- Iniciamos con if dado que algunas ecuaciones van a diferir
%dependiendo si la junta es de revoluta o prismática
if RP(i+1)==0
%Para las juntas de revoluta
%3.3.a- Abrimos una estructura try para tomar en cuenta el caso de
%i=0
try
%3.4.a.- Velocidad angular
w(:,i+1)=R_T(:,:,i+1)*w(:,i)+qp(i+1)*Z_h(:,i+1);
disp(strcat("Velocidad angular w^",i_str,"_",i_str))
disp(w(:,i+1))
%3.5.a.- Aceleración angular

11
 disp(strcat("Aceleración angular wp^",i_str,"_",i_str))
wp(:,i+1)=R_T(:,:,i+1)*(wp(:,i)+cross(w(:,i),qp(i+1)*...
Z_h(:,i+1)))+qpp(i+1)*Z_h(:,i+1);
disp(wp(:,i+1))
%3.6.a.- Aceleración lineal
disp(strcat("Aceleración lineal a^",i_str,"_",i_str))
% a(:,i+1) = simplify(R_T(:,:,i+1)*(cross(wp(:,i),P(:,i))+...
% cross(w(:,i),cross(w(:,i),P(:,i)))+a(:,i)));
a(:,i+1) = simplify(R_T(:,:,i+1)*(cross(wp(:,i),P_i(:,i))+ ...
cross(w(:,i),cross(w(:,i),P_i(:,i)))+a(:,i)));
disp(a(:,i+1))
catch
%3.4.a.- Velocidad angular
w(:,i+1)=qp(i+1)*Z_h(:,i+1);
disp(strcat("Velocidad angular w^",i_str,"_",i_str))
disp(w(:,i+1))
%3.5.a.- Aceleración angular
disp(strcat("Aceleración angular wp^",i_str,"_",i_str))
wp(:,i+1)=qpp(i+1)*Z_h(:,i+1);
disp(wp(:,i+1))
%3.6.a.- Aceleración lineal
disp(strcat("Aceleración lineal a^",i_str,"_",i_str))
a(:,i+1) = simplify(R_T(:,:,i+1)*G);
disp(a(:,i+1))
end
elseif RP(i+1)==1
%Para las juntas prismáticas
%3.3.a- Abrimos una estructura try para tomar en cuenta el caso de
%i=0
try
%3.4.b.- Velocidad angular
w(:,i+1)=R_T(:,:,i+1)*w(:,i);
disp(strcat("Velocidad angular w^",i_str,"_",i_str))
disp(w(:,i+1))
%3.5.b.- Aceleración angular
disp(strcat("Aceleración angular wp^",i_str,"_",i_str))
wp(:,i+1)=R_T(:,:,i+1)*wp(:,i);
disp(wp(:,i+1))
%3.6.a.- Aceleración lineal
disp(strcat("Aceleración lineal a^",i_str,"_",i_str))
% a(:,i+1) = simplify(R_T(:,:,i+1)*(cross(wp(:,i),P(:,i))...
% +cross(w(:,i),cross(w(:,i),P(:,i)))+a(:,i)));
a(:,i+1) = simplify(R_T(:,:,i+1)*(cross(wp(:,i),P_i(:,i))+...
cross(w(:,i),cross(w(:,i),P_i(:,i)))+a(:,i))) + ...
2*cross(w(:,i+1),qp(i+1)*Z_h(:,i+1))+qpp(i+1)*Z_h(:,i+1);
disp(a(:,i+1))
catch
%3.4.b.- Velocidad angular
w(:,i+1)=[0;0;0];
disp(strcat("Velocidad angular w^",i_str,"_",i_str))
disp(w(:,i+1))
%3.5.b.- Aceleración angular
disp(strcat("Aceleración angular wp^",i_str,"_",i_str))
wp(:,i+1)=[0;0;0];

12
 disp(wp(:,i+1))
%3.6.b.- Aceleración lineal
disp(strcat("Aceleración lineal a^",i_str,"_",i_str))
a(:,i+1) = R_T(:,:,i+1)*G+qpp(i+1)*Z_h(:,i+1);
disp(a(:,i+1))
end
else
%En caso de que haya habido un error al declarar la configuración
error("Error al declarar la configuración del robot")
end
%A partir del paso 3.7 lo demás es igual para todas las juntas
%3.7.- Aceleración lineal en el centro de masas
disp(strcat("Aceleración lineal a_c^",i_str,"_",i_str))
a_c(:,i+1) = simplify(cross(wp(:,i+1),Pc(:,i+1))+ ...
cross(w(:,i+1),cross(w(:,i+1),Pc(:,i+1)))+a(:,i+1));
disp(a_c(:,i+1))
%3.8.- Fuerzas generadas por las aceleraciones
disp(strcat("Fuerza F^",i_str,"_",i_str))
F_i(:,i+1) = m(i+1)*a_c(:,i+1);
disp(F_i(:,i+1))
%3.9.- Momentos de torsión generados por el movimiento
disp(strcat("Momento de torsión N^",i_str,"_",i_str))
N_i(:,i+1) = I_c(:,:,i+1)*wp(:,i+1)+...
cross(w(:,i+1),I_c(:,:,i+1)*w(:,i+1));
disp(N_i(:,i+1))
%3.10.- Mostramos la aceleración lineal vista desde el marco de
%referencia inercial
disp(strcat("Aceleración lineal de la articulación ",i_str, " vista desde 0"))
a0(:,i+1) = R0(:,:,i+1)*a(:,i+1);
disp(a0(:,i+1))

end

Velocidad angular w^1_1

Aceleración angular wp^1_1

Aceleración lineal a^1_1

Aceleración lineal a_c^1_1

13
Fuerza F^1_1

Momento de torsión N^1_1

Aceleración lineal de la articulación 1 vista desde 0

Velocidad angular w^2_2

Aceleración angular wp^2_2

Aceleración lineal a^2_2

Aceleración lineal a_c^2_2

14
Fuerza F^2_2

Momento de torsión N^2_2

Aceleración lineal de la articulación 2 vista desde 0

15
Velocidad angular w^3_3

Aceleración angular wp^3_3

Aceleración lineal a^3_3

Aceleración lineal a_c^3_3

16
Fuerza F^3_3

Momento de torsión N^3_3

Aceleración lineal de la articulación 3 vista desde 0

17
4.- Iteraciones entrantes
%4.1.- Generamos un ciclo for que vaya desde 0 a GDL-1
for i=0:(GDL-1)
%4.2.- Declaramos la variable j, que será para las iteraciones
%salientes
j=GDL-i;
j_str=num2str(j);
disp(strcat("Iteración ",j_str))
try
%4.3.a- Calculamos la fuerza neta para cada articulación
f_i(:,j) = simplify(R(:,:,j+1)*f_i(:,j+1)+F_i(:,j));
disp(strcat("Fuerza neta f^",j_str,"_",j_str))
disp(f_i(:,j))
%4.4.a- Calculamos el momento de torsión neto
n_i(:,j) = simplify(N_i(:,j)+R(:,:,j+1)*n_i(:,j+1)...
+cross(Pc(:,j),F_i(:,j))+cross(P_i(:,j),R(:,:,j+1)*f_i(:,j+1)));
disp(strcat("Momento de torsión neto n^",j_str,"_",j_str))
disp(n_i(:,j))

18
 catch
%4.3.b- Calculamos la fuerza neta para cada articulación
f_i(:,j) = simplify(F_i(:,j));
disp(strcat("Fuerza neta f^",j_str,"_",j_str))
disp(f_i(:,j))
%4.4.b- Calculamos el momento de torsión neto
n_i(:,j) = simplify(N_i(:,j)+ cross(Pc(:,j),F_i(:,j)));
disp(strcat("Momento de torsión neto n^",j_str,"_",j_str))
disp(n_i(:,j))
end
%4.5.- Mediante un if armamos el vector de fuerzas generalizadas
if RP(j)==0
%Si es junta de revoluta pasa el momento de torsión
F(j,1) = n_i(:,j).'*Z_h(:,j);
else
%Si es junta prismática pasa la fuerza
F(j,1) = f_i(:,j).'*Z_h(:,j);
end
end

Iteración 3
Fuerza neta f^3_3

Momento de torsión neto n^3_3

19
Iteración 2
Fuerza neta f^2_2

20
Momento de torsión neto n^2_2

21
Iteración 1
Fuerza neta f^1_1

Momento de torsión neto n^1_1

22
%4.6.- Desplegamos el vector de fuerzas generalizadas
disp("Vector de fuerzas generalizadas")

Vector de fuerzas generalizadas

disp(F)

23
24