%% **************************************************************************
%Robotica industrial - Aula 3 - 01/10/2009
%Luis Rodrigues - 36386
%**************************************************************************

clear all, clc
%% Exercicio 3 
%Representar um manipulador com 3 eixos e seus sistemas de coordenadas

%% Objectos para representar sistemas de coordenadas
%% de forma não ambígua. Possibilidade de usar um 
%% objecto único para os 3 eixos.  V. Santos, Oct 2007.
%%


%% Definição parcial de cada sub-objecto (eixo)
Xpart = [
 0  0.05   0.5   0.4  0.5  0.6   0.7  0.8  0.7   1     1    1.5 1   1     0.7  0.8  0.7  0.6  0.5  0.4  0.5  0.05
 0 -0.05  -0.05 -0.2 -0.2 -0.05 -0.2 -0.2 -0.05 -0.05 -0.2  0   0.2 0.05  0.05 0.2  0.2  0.05 0.2  0.2  0.05 0.05
 0  0      0     0    0    0     0     0   0     0      0   0   0   0     0      0    0    0   0    0    0    0
 ];

Ypart = [
0 0.05 0.05 0.2 0.2 0.05 0.05 0.2 0   -0.2 -0.05 -0.05 -0.2 -0.2 -0.05 -0.05
0 0.05 0.6  0.7 0.8 0.7  1    1   1.5 1     1     0.7   0.8  0.7  0.6   0.05
0 0    0    0   0   0    0    0   0   0     0     0     0    0    0     0 
 ];

Zpart = [
     0   0     0      0     0    0    0     0     0     0   0   0   0    0    0   0    0    0    0     0    0
     0  -0.05 -0.05  -0.2  -0.2 -0.1 -0.1  -0.05 -0.05 -0.2 0   0.2 0.05 0.05 0.2 0.2  0.1  0.1  0.05  0.05 0.05
     0   0.05  0.55   0.45  0.8  0.8  0.62  0.66  1     1   1.5 1   1    0.7  0.8 0.45 0.45 0.62 0.58  0.5  0.05
]; 
 
%% Concatenação dos três objectos num só.
X = [ Xpart(1,:) Ypart(1,:) Zpart(1,:) ];
Y = [ Xpart(2,:) Ypart(2,:) Zpart(2,:) ];
Z = [ Xpart(3,:) Ypart(3,:) Zpart(3,:) ];

P = [X;Y;Z; ones(1,size(X,2))];

%% Ilustração do gráfico 3D com a representação do manipulador e os
%% sistemas de coordenadas
close all 
format short

r=fill3(P(1,:),P(2,:),P(3,:), 'y');
xlabel('X'); ylabel('Y'); zlabel('Z');
grid on; hold on
axis([-1 7 -1 7 -1 7]);
axis square

%Definição de todas as transformações obtidas pelo objecto
T(:,:,1)=Trans3D(500,0,-100);
T(:,:,2)=RotacaoZZ((-45/180)*pi)
T(:,:,3)=RotacaoYY((-20/180)*pi)
T(:,:,4)=RotacaoXX((180/180)*pi);
T(:,:,5)=Trans3D(0,0,200);
% T(:,:,6)=Trans3D(0,3,0);

%0 -Transformação em relação ao eixo global
%1 -Transformação em relação ao eixo local
order = [0 1 1 1 1];

%Representação ou não do sistema de eixos
imag = [0 1 1 0 1];

%Cor do sistema de eixos
color = ['y' 'r' 'b' 'w' 'g'];

%Matriz de transformação inicial
MT=eye(4,4);

for n=1:5
    %Se a transformação for em relação ao eixo local
   if order(n)==1
       MT=MT*T(:,:,n);
   else
       MT=T(:,:,n)*MT;
   end
    PMod=MT*P;
    if (imag(n)==1)
        pause(0.5)
        r=fill3(PMod(1,:),PMod(2,:),PMod(3,:), color(n));
    end 
end

MT

% disp('Rotação em torno de YY:')
% b = ( atan2( MT(1,3), MT(1,1)) )*180/pi 
% 
% disp('Rotação em torno de XX:')
% c = ( atan2(MT(3,2), MT(2,2)) )*180/pi
% 
% disp('Rotação em torno de ZZ:')
% a = ( atan2( -MT(1,2)*sin(c), MT(3,2) ))*180/pi

rotX = ( atan2( MT(3,2), MT(3,3)) )*180/pi 
rotZ = ( atan2(MT(2,1), MT(1,1)) )*180/pi
rotY = ( atan2( -MT(3,1), MT(1,1)*cos(rotZ)+MT(2,1)*sin(rotZ)) )*180/pi

rpy=RPY(30/180*pi,-10/180*pi,180/180*pi);

Pr=rpy*P;
r=fill3(Pr(1,:),Pr(2,:),Pr(3,:), 'k');

rotX = ( atan2( rpy(3,2), rpy(3,3)) )*180/pi 
rotZ = ( atan2(rpy(2,1), rpy(1,1)) )*180/pi
rotY = ( atan2( -rpy(3,1), rpy(1,1)*cos(rotZ)+rpy(2,1)*sin(rotZ)) )*180/pi