%----------------------------------------------------------------------
%                             Projecto final
% 
%                    Luís Alberto Soares Rêgo nº. 20008 
%              Renato Francisco dos Santos Barbosa n.º 18382
%                         Engenharia Mecânica
%                       
%                 Cinematica directa das pernas do robot
%----------------------------------------------------------------------
%
% Função ROBPLOT - Desenho gráfico e animaçao do robô a partir da descrição cinemática DH 
%                  e das trajectórias das juntas q; desenha o sistema de coordenadas 
%                  associado ao elemento terminal para mostrar a posiç~ao final das pernas 



function [CMJuntas,CMTotal,tL,tR,CML,CMR,Pf] = ROBPLOT(Duas_Pernas,Perna_Tronco,Q,Qf)


% N.º de ciclos para a animaçao:

NN = 3;

% Inicicializaçao dos torques a zero nas diferentes juntas:
M1L = 0;
M2L = 0;
M3L = 0;
M4L = 0;
M5L = 0;
M5R = 0;
M4R = 0;
M3R = 0;
M2R = 0;
M1R = 0;
CM1L = 0;
CM2L = 0;
CM3L = 0;
CM4L = 0;
CM5L = 0;
CM5R = 0;
CM4R = 0;
CM3R = 0;
CM2R = 0;
CM1R = 0;

% Transformaçao dos angulos de junta de graus em radianos:
Q = Q*pi/180;
Qf = Qf*pi/180;


% Animaçao:
for j = 1:NN
    
    Q(1) = Q(1) + j*((Qf(1)-Q(1))/NN);
    Q(2) = Q(2) + j*((Qf(2)-Q(2))/NN); 
    Q(3) = Q(3) + j*((Qf(3)-Q(3))/NN);
    Q(4) = Q(4) + j*((Qf(4)-Q(4))/NN);
    Q(5) = Q(5) + j*((Qf(5)-Q(5))/NN);
    Q(6) = Q(6) + j*((Qf(6)-Q(6))/NN);
    Q(7) = Q(7) + j*((Qf(7)-Q(7))/NN);
    Q(8) = Q(8) + j*((Qf(8)-Q(8))/NN);
    Q(9) = Q(9) + j*((Qf(9)-Q(9))/NN);
    Q(10) = Q(10) + j*((Qf(10)-Q(10))/NN);
    
    Q = [Q(1) Q(2) Q(3) Q(4) Q(5) Q(6) Q(7) Q(8) Q(9) Q(10)];
    
    [RTG1,Pf1] = CINDIR(Perna_Tronco,Q);  % Chama-se a funçao CINDIR2 que permite o calculo dos pontos a desenhar, 
                                           % a partir da transformaçao
                                           % efectuada, considerando aqui a existencia do tronco.
                                          
    [RTG,Pf] = CINDIR(Duas_Pernas,Q); % Chama-se a funçao CINDIR que permite o calculo dos pontos a desenhar, 
                                      % a partir da transformaçao efectuada, para as pernas do robot.                          
                                
% DETERMINAÇAO DOS BINARIOS E DOS CENTROS DE MASSA PARA AS DIFERENTES JUNTAS:
% Dados:
m0 = 0.28;
m1 = 0.20;
m2 = 0.63;
m3 = 0.26;
m4 = 0.21;
m5 = 4;
g = 9.81;
Lo = [0;0;12.5];

% Determinaçao dos centros de massa dos elos:
CM = [(((Pf(1,2:11)-Pf(1,1:10))/2)+Pf(1,1:10));(((Pf(2,2:11)-Pf(2,1:10))/2)+Pf(2,1:10));(((Pf(3,2:11)-Pf(3,1:10))/2)+Pf(3,1:10))];
CM3 = (Pf1(:,7));
CMTotal = [Lo(:,1) CM(:,1) CM(:,2) CM(:,3) CM(:,4) CM3(:,1) CM(:,6) CM(:,7) CM(:,8) CM(:,9) CM(:,10)];



% Determinaçao das coordenadas do centro de massa total:
CMJunta1 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,2)+CMTotal(:,10)))+(m2*(CMTotal(:,3)+CMTotal(:,9)))+(m3*(CMTotal(:,4)+CMTotal(:,8)))+(m4*(CMTotal(:,5)+CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(2*m0+2*m1+2*m2+2*m3+2*m4+m5));
CMJunta2 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,3)+CMTotal(:,9)))+(m3*(CMTotal(:,4)+CMTotal(:,8)))+(m4*(CMTotal(:,5)+CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(m0+m1+2*m2+2*m3+2*m4+m5));
CMJunta3 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,4)+CMTotal(:,8)))+(m4*(CMTotal(:,5)+CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(m0+m1+m2+2*m3+2*m4+m5));
CMJunta4 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,8)))+(m4*(CMTotal(:,5)+CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(m0+m1+m2+m3+2*m4+m5));
CMJunta5 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,8)))+(m4*(CMTotal(:,7)))+(m5*(CMTotal(:,6))))/(m0+m1+m2+m3+m4+m5));
CMJunta6 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,8)))+(m4*(CMTotal(:,7))))/(m0+m1+m2+m3+m4));
CMJunta7 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9)))+(m3*(CMTotal(:,8))))/(m0+m1+m2+m3));
CMJunta8 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10)))+(m2*(CMTotal(:,9))))/(m0+m1+m2));
CMJunta9 = (((m0*(CMTotal(:,11)))+(m1*(CMTotal(:,10))))/(m0+m1));
CMJunta10 = ((m0*(CMTotal(:,11)))/(m0));

CMJuntas = [CMJunta1 CMJunta2 CMJunta3 CMJunta4 CMJunta5 CMJunta6 CMJunta7 CMJunta8 CMJunta9 CMJunta10];

CM1L = horzcat(CM1L,CMJuntas(2,1));
CM2L = horzcat(CM2L,CMJuntas(1,2));
CM3L = horzcat(CM3L,CMJuntas(1,3));
CM4L = horzcat(CM4L,CMJuntas(1,4));
CM5L = horzcat(CM5L,CMJuntas(2,5));

CM5R = horzcat(CM5R,CMJuntas(2,6));
CM4R = horzcat(CM4R,CMJuntas(1,7));
CM3R = horzcat(CM3R,CMJuntas(1,8));
CM2R = horzcat(CM2R,CMJuntas(1,9));
CM1R = horzcat(CM1R,CMJuntas(2,10));
plot3(CMJuntas(1,1),CMJuntas(2,1),0,'ro')
hold on

% Calculo dos momentos, em N.m:

% Braços de Força:

de_1L = sqrt((CMJuntas(2,1)-Pf(2,1))^2+(CMJuntas(3,1)-Pf(3,1))^2);
br_1L = sqrt(de_1L^2 - (CMJuntas(3,1)-Pf(3,1))^2);
de_2L = sqrt((CMJuntas(1,2)-Pf(1,2))^2+(CMJuntas(3,2)-Pf(3,2))^2);
br_2L = sqrt(de_2L^2 - (CMJuntas(3,2)-Pf(3,2))^2);
de_3L = sqrt((CMJuntas(1,3)-Pf(1,3))^2+(CMJuntas(3,3)-Pf(3,3))^2);
br_3L = sqrt(de_3L^2 - (CMJuntas(3,3)-Pf(3,3))^2);
de_4L = sqrt((CMJuntas(1,4)-Pf(1,4))^2+(CMJuntas(3,4)-Pf(3,4))^2);
br_4L = sqrt(de_4L^2 - (CMJuntas(3,4)-Pf(3,4))^2);
de_5L = sqrt((CMJuntas(2,5)-Pf(2,5))^2+(CMJuntas(3,5)-Pf(3,5))^2);
br_5L = sqrt(de_5L^2 - (CMJuntas(3,5)-Pf(3,5))^2);

% Momentos da Perna Esquerda:
t1L = ((m0+2*m1+2*m2+2*m3+2*m4+m5) * g *  (br_1L * 0.001));
t2L = ((m0+m1+2*m2+2*m3+2*m4+m5) * g * (br_2L * 0.001));
t3L = ((m0+m1+m2+2*m3+2*m4+m5) * g * (br_3L * 0.001));
t4L = ((m0+m1+m2+m3+2*m4+m5) * g * (br_4L * 0.001));
t5L = ((m0+m1+m2+m3+m4+m5) * g * (br_5L * 0.001));

% Braços de força:

de_1R = sqrt((CMJuntas(2,10)-Pf(2,10))^2+(CMJuntas(3,10)-Pf(3,10))^2);
br_1R = sqrt(de_1R^2 - (CMJuntas(3,10)-Pf(3,10))^2);
de_2R = sqrt((CMJuntas(1,9)-Pf(1,9))^2+(CMJuntas(3,9)-Pf(3,9))^2);
br_2R = sqrt(de_2R^2 - (CMJuntas(3,9)-Pf(3,9))^2);
de_3R = sqrt((CMJuntas(1,8)-Pf(1,8))^2+(CMJuntas(3,8)-Pf(3,8))^2);
br_3R = sqrt(de_3R^2 - (CMJuntas(3,8)-Pf(3,8))^2);
de_4R = sqrt((CMJuntas(1,7)-Pf(1,7))^2+(CMJuntas(3,7)-Pf(3,7))^2);
br_4R = sqrt(de_4R^2 - (CMJuntas(3,7)-Pf(3,7))^2);
de_5R = sqrt((CMJuntas(2,6)-Pf(2,6))^2+(CMJuntas(3,6)-Pf(3,6))^2);
br_5R = sqrt(de_5R^2 - (CMJuntas(3,6)-Pf(3,6))^2);

% Momentos da Perna Direita:
t5R = ((m0+m1+m2+m3+m4) * g * (br_5R * 0.001));
t4R = ((m0+m1+m2+m3) * g * (br_4R * 0.001));
t3R = ((m0+m1+m2) * g * (br_3R * 0.001));
t2R = ((m0+m1) * g * (br_2R * 0.001));
t1R = ((m0) * g * (br_1R * 0.001));

t_L = [t1L; t2L; t3L; t4L; t5L];
t_R = [t5R; t4R; t3R; t2R; t1R];

% Concatenaçao dos momentos para as diferentes posiçoes da animaçao:
M1L = horzcat(M1L,t_L(1,1));
M2L = horzcat(M2L,t_L(2,1));
M3L = horzcat(M3L,t_L(3,1));
M4L = horzcat(M4L,t_L(4,1));
M5L = horzcat(M5L,t_L(5,1));

M5R = horzcat(M5R,t_R(1,1));
M4R = horzcat(M4R,t_R(2,1));
M3R = horzcat(M3R,t_R(3,1));
M2R = horzcat(M2R,t_R(4,1));
M1R = horzcat(M1R,t_R(5,1));


% DESENHO DA BASE DOS PES:
Pe_direito = [-50 ,-50 , 90 , 90 , -50 
              -35 , 35 , 35  , -35 , -35      
                0 ,  0 , 0   ,  0  ,  0 ];
Desfill3(Pe_direito,'k')
% hold on
Pe_esquerdo =[ 0    ,  0   ,  0   ,  0  
              -35   ,  35  ,  35  , -35 
              -50   , -50  , 90  , 90   
               1    ,   1  ,   1  ,   1 ];

Pe_esquerdo1 = RTG*Pe_esquerdo;
Desfill3(Pe_esquerdo1,'k')

% DESENHO DOS ELOS E JUNTAS DAS PERNAS:
                           
% Desplot3(Pf,'b')
plot3(Pf(1,:),Pf(2,:),Pf(3,:),'b', 'linewidth', 1.5)    
P_inicial = [0,Pf(1,1)
             0,Pf(2,1)
             0,Pf(3,1)];
    
plot3(P_inicial(1,:),P_inicial(2,:),P_inicial(3,:),'b')
plot3(Pf(1,1:10),Pf(2,1:10),Pf(3,1:10),'go', 'linewidth', 1.5)
% Desplot3(Pf1,'b')
plot3(Pf1(1,:),Pf1(2,:),Pf1(3,:),'b', 'linewidth', 1.5)    
plot3(Pf1(1,7),Pf1(2,7),Pf1(3,7),'go', 'linewidth', 1.5)
 
axis([-250,250,-250,250,0,450])
view(45,30)
grid on
Title('Cinematica directa das pernas do robot humanoide')
XLabel('Eixo dos XX');
YLabel('Eixo dos YY');
ZLabel('Eixo dos ZZ');  
pause(0.005)
hold off

% mov(:,j) = getframe(figure(1))
end

% Momentos Finais obtidos:
tL = [M1L; M2L; M3L; M4L; M5L];
tR = [M5R; M4R; M3R; M2R; M1R];
CML = [CM1L; CM2L; CM3L; CM4L; CM5L];
CMR = [CM1R; CM2R; CM3R; CM4R; CM5R];
% movie2avi(mov,'Cinematica_Directa_Pernas_New');

