CINEMÁTICA DEL ROBOT MENTOR Y ESCRITURA DE LETRAS

Valeria Staiti
2021/2022
 CINEMATICA DIRECTA
La cinemática directa se apoya en establecer cuál es la posición y orientación del extremo
final del robot, con en relación con un sistema de coordenadas , conocidos los valores de
las articulaciones y los parámetros geométricos de los recursos del robot.

Aquì se anexa la foto del robot en el que se vean los sistemas de referencia generados

De seguido se muestra la tabla de los parametros DH del robot Mentor

Los ángulos que hay que introducir en las tablas del Matlab son en radianes, pero en el
programa Walli4 en ángulos walli.

El ángulo en walli será igual a la siguiente relación:

Donde: qw (ángulos walli), qg (ángulos en grados), w1 y 2 (walli en el punto 1 y/o 2) y g1

(ángulos grados en el punto 1)

El programa que se ha desarollado por esta pratica nos calcula la Matriz de Transformación
Homogénea correspondiente a los valores de los ángulos de rotación del robot. Para ello
hemos creados dos funciones, denavit y mentor_dir.

Denavit: Función que calcula la Matriz de Transformación para cada valor de án gulos del
robot. La matriz que relaciona el eslabón i con el i-1 viene dada por la matriz:

Mentor_ dir: Nos calcula la Matriz de Transformación Homogénea, “Pose”, desde la base
del robot hasta su extremo, haciendo las correspondientes transformaciones de ángulos

CINEMATICA INVERSA
El fin de la cinemática inversa de un robot, es calcular como tienen que estar sus
articulaciones, para que el extremo del robot tenga la posición y orientación deseadas.

La forma de indicar la posición y orientación es por medio de una matriz de transformación

homogénea. Por otro lado, a la inversa de lo cual pasa con la cinemática directa en que si
se sabe la posición de las articulaciones se sabe de manera inequívoca la posición y
orientación del extremo del robot con respecto a la base. En la cinemática inversa esto no
pasa, ya que el robot puede tener diversas configu raciones, en esta práctica sólo se van a
tener en cuenta la configuración del codo

De seguido se esplico como calcular la cinematica inversa de las tres primeras

articulaciones.

Se tiene que aplicar entonces el desacoplo cinematico, que es un procedimiento que

permite de determinar la posición de la muñeca del robot conociendo la de su extremo .

Por deducir las equaciones de las dos ultimas articulaciones:

El programa con la funcion q=mentor_inv(T,codo,formato) tiene los siguientes parámetros
son:

q – es la variable de salida, y es la posición de cada una de las articulaciones en el formato

correspondiente, para que el extremo del robot esté en la posición y orientación deseada .

T – Matriz de transformación homogénea que indica la posición y orientación del extremo

del robot con respecto a la base que se desea obtener.

Codo – Indica la solución de la cinemática inversa que se desea en función del codo.
Existen dos soluciones codo en posición alta ‘a’ y en posición baja ‘b’.

Formato – Indica en que unidades se desea que se genere la salida, las posibilidades son:
‘g’ en grados, ‘r’ en radianes y ‘w’ en walli.

Por lo tanto el fin es calcular los ángulos de todas las articulaciones, que estan contenido
en un vector q.

ESCRITURA DE LAS LETRAS

Para la escritura de las iniciales ha sido necesario realizar un programa que nos calcules
cada punto de nuestras iniciales, además que para poder introducir los datos en el
programa Walli debemos pasarlo a los ángulos necesarios para que pueda entender las
instrucciones el robot mentor.

El programa lo será llamado letras.m., será realizado en Matlab tomando como origen los
puntos (0,0) la parte inferior central de la inicial del apellido, que coincide con El punto
origen es al que apunta el extremo del robot cuando se le introducen los ángulos walli
siguientes [128, 84, 24, 98, 128]
Teniendo en cuenta las medidas, se ha hecho el dibujo de las letras. Cada segmento de la
lettras se divide en puntos, que se ha tenido en cuenta de cojerlos a una distancia de
maximo 1 cm entre ellos. El formato tiene que ser por ejemplo :

Entonces se declara el punto origine y los incrementos de los puntos respecto a esto. Se va a crear
asì una matriz T y gracias a esta se calcula la cinematica inversa y se obtiene los angulos en walli.

Una vez realizado el programa y recibir las coordenadas en walli, se ha de pasar los datos a
dicho programa, colocando los ángulos de coordenadas en (x,y,z) en las tres primeras
columnas y en las dos últimas los ángulos de translación y rotación necesarios para la
escritura de las letras. Si se quiere que se vaya de un punto a otro sin que el robot escriba,
se deberá aumentar su coordenada z en 20mm.

Teniendo solo un apellido el dibujo conseguido representa las letras VS!

La letras conseguidas no estan bien definidas, siendo antiguo el robot Mentor

PROGRAM A M ATLAB
%clear
%clc
% Mentor Cinemática directa.
% A05 = mentor_dir(q) devuelve la matriz de transformación
del
% primer sistema de coordenadas al último en función del
vector q
% de variables articulares.
%
% See also DENAVIT.
function A05 = mentor_dirC(q,formato)

a1=(255-128)/(105-0); % Calculamos a1 y b1
b1=128-a1*0;
a2=(155-20)/(0+90); % Calculamos a2 y b2
b2=20-a2*(-90);
a3=(255-128)/(115-0); % Calculamos a3 y b3
b3=128-a3*0;
a4=(255-128)/(160-0); % Calculamos a4 y b4
b4=128-a4*0;
a5=(255-128)/(160-0); % Calculamos a5 y b5
b5=128-a5*0;

if formato == 'g' % los datos se han introducido en grados

qg=q % como los datos se han introducido en grados, qg=q
qr=qg*pi/180 % muestra el valor de qr en el command window
qw(1)=qg(1)*a1+b1; % Las constantes a1 y b1 hay que
calcularlas con
% los datos dados en la práctica
qw(2)=qg(2)*a2+b2; % Las constantes a2 y b2 hay que
calcularlas con
% los datos dados en la práctica
qw(3)=qg(3)*a3+b3; % Las constantes a3 y b3 hay que
calcularlas con
% los datos dados en la práctica
qw(4)=qg(4)*a4+b4; % Las constantes a4 y b4 hay que
calcularlas con
% los datos dados en la práctica
qw(5)=qg(5)*a5+b5; % Las constantes a5 y b5 hay que
calcularlas con
% los datos dados en la práctica
qw=round(qw) % redondea y lo muestra en el command window
end

if formato == 'r'
qr=q % como los datos se han introducido en radianes, qr=q
qg=qr*180/pi % como los datos se han introducido en grados,
qg=q
qw(1)=qg(1)*a1+b1; % Las constantes a1 y b1 hay que
calcularlas con
% los datos dados en la práctica
qw(2)=qg(2)*a2+b2; % Las constantes a2 y b2 hay que
calcularlas con
% los datos dados en la práctica
qw(3)=qg(3)*a3+b3; % Las constantes a3 y b3 hay que
calcularlas con
% los datos dados en la práctica
qw(4)=qg(4)*a4+b4; % Las constantes a4 y b4 hay que
calcularlas con
% los datos dados en la práctica
qw(5)=qg(5)*a5+b5; % Las constantes a5 y b5 hay que
calcularlas con
% los datos dados en la práctica
qw=round(qw) % redondea y lo muestra en el command window
end

if formato == 'w'
qw=q % como los datos se han introducido en walli, qw=q
qg(1)=(qw(1)-b1)/a1;
qg(2)=(qw(2)-b2)/a2;
qg(3)=(qw(3)-b3)/a3;
qg(4)=(qw(4)-b4)/a4;
qg(5)=(qw(5)-b5)/a5;
qg
qr=qg*pi/180
end
% Parámetros Denavit-Hartenberg del robot
l1=185;
l2=165;
l3=150;
l4=205;

teta = [pi/2-qr(1) pi/2+qr(2) qr(3) (-pi)/2+qr(4) (-

pi)/2+qr(5)];
d = [l1 0 0 0 l4];
a = [0 l2 l3 0 0];
alfa = [pi/2 0 0 (-pi)/2 0];

% Matrices de transformación homogénea entre sistemas de

coordenadas consecutivos
A01 = denavit(teta(1), d(1), a(1), alfa(1));
A12 = denavit(teta(2), d(2), a(2), alfa(2));
A23 = denavit(teta(3), d(3), a(3), alfa(3));
A34 = denavit(teta(4), d(4), a(4), alfa(4));
A45 = denavit(teta(5), d(5), a(5), alfa(5));

A05 = A01 * A12 * A23 * A34 * A45;

%%Práctica 5.
% mentor_inv cinemática inversa
% Q = mentor_inv(T, codo, formato) devuelve el vector de
coordenadas
% articulares correspondiente a la solución cinemática
inversa de la
% mano
% del manipulador en la posición y orientación expresadas en
la matriz
% T.
% codo = 'a' indica codo del robot arriba, es decir, que la
% articulación 3 se
% sitúa por encima de la articulación 2, mientras que CODO =
'b'
% indica codo
% abajo, es decir que la articulación 2 se sitúa por encima
de la 3.
% formato = los angulos de las articulaciones se devuelven
en: 'r'
% radianes, 'g' grados o 'w' walli.
% See also mentor_dir, DENAVIT.
function qs=mentor_invC(T,codo,formato)
l1=185;
l2=165;
l3=150;
l4=205;

%Parámetros Denavit-Hartenberg del robot.

tita_off_DH = [pi/2 pi/2 0 -pi/2 -pi/2];
d_DH = [l1 0 0 0 l4];
a_DH = [0 l2 13 0 0];
alfa_DH = [pi/2 0 0 -pi/2 0];

%Posición de la muñeca del manipulador.

p=T(1:3,4)-l4*T(1:3,3); %El punto de la muñeca es el punto
del extremo del robot menos l4 por el vector director del eje
z del extremo
%del robot.
px=p(1);
py=p(2);
pz=p(3);

%Solución de la primera articulación: q1.

q1 = atan(px/py);
%Se calcula el valor absoluto de q3.
q3 = acos((px^2+py^2+(pz-l1)^2-l2^2-l3^2)/(2*l2*l3));
%Se calculan los valores absolutos de alfa y beta.
alfa=abs(atan((l3*sin(q3))/(l2+l3*cos(q3))));
beta=abs(atan((pz-l1)/(sqrt(px^2+py^2))));
%Con los valores absolutos de alfa y beta y teniendo en
cuenta si el
%codo está arriba o abajo y si pz<l1 o pz >li se calcula q2
y el signo de
%q3.
if codo=='a' %Codo arriba
if pz>l1
q2=-pi/2+beta+alfa;
q3=-abs(q3);
else
q2=-pi/2-beta+alfa;
q3=-abs(q3);
end
else %Codo abajo
if pz>l1
q2=-pi/2+beta-alfa;
q3=abs(q3);
else
q2=-pi/2-beta-alfa;
q3=abs(q3);
end
end
%Cálculo de la matriz de transformación A03.
A01 = denavit(tita_off_DH(1)-q1, d_DH(1), a_DH(1),
alfa_DH(1));
A12 = denavit(tita_off_DH(2)+q2, d_DH(2), a_DH(2),
alfa_DH(2));
A23 = denavit(tita_off_DH(3)+q3, d_DH(3), a_DH(3),
alfa_DH(3));
A03 = A01 * A12 * A23;
R03=A03(1:3,1:3); %Matriz de rotación de las tres primeras
articulaciones

R=T(1:3,1:3); %Matriz de rotación que se desea obtener

R_i=R03'*R; %Producto de la inversa de la matriz R03 y de R,
es la base
%para calcular las dos últimas ecuaciones de acuerdo con las
ecuaciones
%vistas en clase.
q4=asin(R_i(2,3));
q5=asin(R_i(3,2))*(-1);
%Vector de coordenadas articulares.
q = [q1 q2 q3 q4 q5];

a1=(255-128)/(105-0);
b1=128-a1*0;
a2=(155-20)/90;
b2=20-a2*(-90);
a3=(255-128)/115;
b3=128-a3*0;
a4=(255-128)/160;
b4=128-a4*0;
a5=a4;
b5=128-a5*0;

qr=q; % el matlab hace sus cálculos en radianes, por tanto

no hay que hacer conversión
qg=qr*180/pi; % muestra el valor de qg en el command window
qw(1)=qg(1)*a1+b1; % Las constantes a1 y b1 hay que
calcularlas con
% los datos dados en la práctica
qw(2)=qg(2)*a2+b2; % Las constantes a2 y b2 hay que
calcularlas con
% los datos dados en la práctica
qw(3)=qg(3)*a3+b3; % Las constantes a3 y b3 hay que
calcularlas con
% los datos dados en la práctica
qw(4)=qg(4)*a4+b4; % Las constantes a4 y b4 hay que
calcularlas con
% los datos dados en la práctica
qw(5)=qg(5)*a5+b5; % Las constantes a5 y b5 hay que
calcularlas con
% los datos dados en la práctica
qw=round(qw);

if formato == 'g' %Los datos se devuelven en grados

qs=qg; %Se asigna la salida de la función en grados
end
if formato == 'r'
qs=qr;
end
if formato == 'w'
qs=qw;
end

%Escritura de mis iniciales

%Punto de origen dado por la matriz A05 que hemos calculado
al meter los
%ángulos del robot en la función mentor_dir.
Pox=0; % A05 (1,4) ;
Poy= 217;
Poz= -25;
%Medidas en mm.
%inc=[inc_Px inc_Py inc_Pz]
inc=[ 0 0 20; %0
-34 35 20; %1 Letra V
-34 35 0; %2
-32 25 0; %3
-31 15 0; %4
-30 5 0; %5
-29 15 0; %6
-28 25 0; %7
-26 35 0; %8
-26 35 20; %fine lettera v
10 35 20; %inizio lettera s
10 35 0; %11
0 35 0; %12
-10 35 0; %13
-10 25 0; %14
-10 15 0; %15
0 15 0; %l6
10 15 0; %17
10 5 0; %18
0 5 0; %19
-10 5 0; %20
-10 5 20; %fine lettera s
25 35 20; %inizio !
25 35 0; %23
25 25 0; %24
 25 15 0; %25
25 15 20; %26
25 5 20; %27
25 5 0; %28
25 5 20]; %fine del punto

longitud = length(inc);
Q = zeros(1);
for i=1:longitud
inc_Px = inc(i,1);
inc_Py = inc(i,2);
inc_Pz = inc(i,3);

T=[1 0 0 Pox+inc_Px; 0 -1 0 Poy+inc_Py; 0 0 -1

Poz+inc_Pz; 0 0 0 1];
S = mentor_inv(T,'a','w');

Q(i,1) = i;
Q(i,2) = S(1);
Q(i,3) = S(2);
Q(i,4) = S(3);
Q(i,5) = S(4);
Q(i,6) = 128;
end