UNIVERSIDAD DE BUENOS AIRES

FACULTAD DE INGENIERA

67.43 Robtica Industrial

Trabajo Prctico N 1

Profesor : Ing. Anigstein Mauricio

Jefe de TP : Ing. Pablo Sebastin Gonzlez

Alumno: Crescentini Gustavo

N Padrn: 85131
Cuat: 1
Ao: 2017
Robotica TP1

Ejercicio 1:

Dada la Matriz R, obtener el cuaternion Q

1 0 0
=[ 0 0 1]
0 1 0

Para el cuaternion de rotacion = [ ; , , ] la matriz correspondiente R ser:

22 + 2 2 1 2 2 2 + 2
2 2
= [ ] = [ 2 + 2 2 + 2 1 2 2 ]
2 2 2 + 2 22 + 2 2 1

A partir de ello se desarrolla la funcin Cuaternion en Matlab tal que devuelve el resultado

1 0 0
=[ 0 0 1] = [0; 0 , 0.7071, 0.7071]
0 1 0
function [ Q ] = Cuaternion( R )
%Dada la matriz R de rotacion obtiene el cuaternion Q
n = R(:,1);
s = R(:,2);
a = R(:,3);

g = sqrt(n(1)+s(2)+a(3)+1)/2;
if g ~= 0
qx = sign(s(3)-a(2))*(sqrt(n(1)-s(2)-a(3)+1)/2);
qy = sign(s(3)-a(2))*(sqrt(n(1)-s(2)-a(3)+1)/2);
qz = sign(s(3)-a(2))*(sqrt(n(1)-s(2)-a(3)+1)/2);
else
qx = sqrt(n(1)-s(2)-a(3)+1)/2;
qy = sqrt(s(2)-n(1)-a(3)+1)/2;
qz = sqrt(a(3)-s(2)-n(1)+1)/2;
[maximo, ind] = max([qx;qy;qz]);
switch ind
case 1
qy = qy*sign(n(2)+s(1));
qz = qz*sign(n(3)+a(1));
case 2
qx = qx*sign(n(2)+s(1));
qz = qz*sign(s(3)+a(2));
case 3
qy = qy*sign(s(3)+a(2));
qx = qx*sign(n(3)+a(1));
end
end
Q = [g qx qy qz];
end
 Ejercicio 2:

A partir del Grafico hallar: A01 y A02 y luego calcular A12

Z0 2
Z1
Y0
X1
X0
2
Y2 Y1

X2

Z2

1 0 0 0
10 = [ 0 0 1 2]
0 1 0 0
0 0 0 1
0 1 0 2 1 0 0 0 0 1 0 2
20 =[1 0 0 2] [0 1 0 0 ] = [1 0 0 2]
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

Para obtener A12 se calcula mediante (A01)-1* A02 = A12

1 0 0 0
1 0 0 1 0
10 =[ ]
0 1 0 2
0 0 0 1
1 0 0 0 0 1 0 2 0 1 0 2
0 0 1 0 1 0 2 0 0 1 0
12 =[ ][ ]=[ ]
0 1 0 2 0 0 1 0 1 0 0 0
0 0 0 1 0 0 0 1 0 0 0 1
Ejercicio 3

Realizar un programa que calcule el problema inverso de los angulos de Euler Z, Y, Z y

buscando actual

function [phi, tita, psi] = Eulerinverso

Raddeg = 180/pi; %constante de pasaje de radianes a decimales
Conf = 0;
% ingreso la matriz de de rotaciones a realizar el inverso
R= input('Ingrese el valor de la matriz R: ');
phi = input('Ingrese el valor de la Phi actual(grados): ');
phi = phi/Raddeg;
while 1 %repite hasta que se ingresen valores validos de
configuracion
Conf = input ('Ingrese los valores de configuracin [1 o -
1]:');
if (Conf == 1)||(Conf == -1)
break;
end
end

n = R(:,1);
s = R(:,2);
a = R(:,3);
if a(1)==0
if a(2)==0
tita = 0;
phi = phi;
else
phi = sign(a(2))*pi/2;
tita = atan2(a(1)*cos(phi)+a(2)*sin(phi),a(3));
end
psi = atan2 (-n(3)*sin(phi)+n(2)*cos(phi),-s(1)*sin(phi)-
s(2)*cos(phi));
else
if a(2)==0
if a(1)>0
phi = 0;
else
phi = pi;
end
else
phi = atan2(Conf*a(2),sign(a(2))*sign(a(1))*Conf*a(1));
end
tita = atan2(a(1)*cos(phi)+a(2)*sin(phi),a(3));
psi = atan2 (-n(3)*sin(phi)+n(2)*cos(phi),-s(1)*sin(phi)-
s(2)*cos(phi));
end

phi = phi *Raddeg

psi = psi *Raddeg
tita = tita *Raddeg
end