%--------------------------------------------------------------------------------------
% Ficheiro:		Sim_JointSP.m
%
% Autor: 		Filipe Silva
% Data:			31-Maio -2004 
% Modificado:   26-Março-2007    
% Objectivo:	Animação cinemática do movimento do robo bipede 12dof-3D
%--------------------------------------------------------------------------------------

clear all
close all
clc
%warning off

%--------------------------------------------------------------------------------------
% Inicializaçao de constantes e variaveis
%--------------------------------------------------------------------------------------
 %__ Parametros do robo humanoide -----------------------------------------------------
 DefineRob
 %__ Aceleraçao da gravidade ----------------------------------------------------------
 go=9.8;
 %__ Outras constantes ---------------------------------------------------------------- 
 DIM = 24;                      %-- Dimensão do vector tempo de simulaçao (Tsim) ------
 XYZRef = [0 0 0];              %-- Coordenadas de referencia -------------------------
 SS = 1; DS = 0;                %-- Definiçao das fases de SS e DS --------------------
 RL = 1; LL = 0;                %-- Perna de referencia -------------------------------
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Representação gráfica do robô na configuração inicial: POSIÇAO ZERO 
%--------------------------------------------------------------------------------------
qi = [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]; %-- Postura vertical ------------------
InitRob
%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% MOVETYPE: "FreeStyle"
%--------------------------------------------------------------------------------------            
 qf = [ pi/6 0 0 0 -pi/6 0 ,   0     pi/6 0 0 0 -pi/6 , 0 0 0 0 0 0 0 pi/6;  ...
       -pi/6 0 0 0  pi/6 0 ,   0    -pi/6 0 0 0  pi/6 , 0 0 0 0 0 -pi/6 0 0; ...
        0    0 0 0  0    0 ,   0     0    0 0 0 0     , 0 0 0 0 0 0 0 0; ...
        0    0 0 0  0    0 ,   0     0    0 0 0 0     , 0 pi/3 0 pi/3 -pi/3 0 pi/3 0; ...
        0    0 0 0  0    0 ,   0     0    0 0 0 0     , 0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------
% MOVETYPE: Rodar sobre si proprio -> descriçao da tarefa no espaço das juntas
% Parametros de entrada: ang_i, angulo de inclinaçao laeral da perna direita
%                        ang_r, angulo de rotaçao do pe esquerdo
%                        Hh, altura da anca depende de ang_i, mas devo dobrar joelhos!
%--------------------------------------------------------------------------------------
  ang_iR = pi/8;                ang_r  = pi/10;          
%-- Valores maximos de Hh e D2, para dado ang_iR, com a perna totalmente esticada -----
  Lt    = L(2)+L(3)+L(4)+L(5);  Hh_max = Lt*cos(ang_iR);  D2_max = Lt*sin(ang_iR);
%-- Especificar altura da anca desejada (dobrar joelhos) e elevaçao do pe -------------
  Hh    = 0.90*Hh_max;          Fc     = 0.0375;
%--------------------------------------------------------------------------------------  
% Partindo da posiçao zero (q = 0):
% 1) Baixar a anca para Hh (i.e., dobrar os joelhos) na vertical
%--------------------------------------------------------------------------------------   
  [q2,q3] = CinInv2R( [Hh/cos(ang_iR)-L(2)-L(5) 0],[L(3) L(4)],[1 -1] );
  qf = [ qf ; ...
         0  q2  q3  -q2-q3  0  0  ,  0  0  -q2-q3  q3  q2  0  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 2) Transferir o CoGx,y para o pe direito: movimento lateral de ang_iR
%--------------------------------------------------------------------------------------  
  qf = [ qf ; ...
         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  0  ang_iR  -q2-q3  q3  q2  -ang_iR  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 3) Elevar o pe esquerdo (Fc em ZZ) ao longo do plano definido pela perna
%--------------------------------------------------------------------------------------  
  [q9,q10] = CinInv2R( [(Hh-Fc)/cos(ang_iR)-L(2)-L(5) 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
  qf = [ qf ; ...
         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  0  ang_iR  q9  q10  -q9-q10  -ang_iR  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 4) Rodar o pe esquerdo de um angulo ang_r em torno de eixo vertical (q7)
%--------------------------------------------------------------------------------------  
%  qf = [ qf ; ...
%         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  -ang_r  ang_iR  q9  q10  -q9-q10  -ang_iR  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 5) Alinhar a perna esquerda com o seu novo plano: ang_iL > ang_iR
% ATENÇAO: sera possivel juntar os passos 4 e 5 num unico passo!
%--------------------------------------------------------------------------------------  
  D2 = Hh*tan(ang_iR);  D1 = L(8)-D2; %-- Distancias entre cada pe e a anca esquerda
  L_aux = -D1*cos(ang_r)+sqrt(L(8)^2-D1^2*sin(ang_r)^2); %-- Distancia anca-pe esquerdos 
  if L_aux > D2_max, errordlg('Error: redefine Hh','Error message'), end

  ang_iL = atan2(L_aux,Hh); %-- ANG_iRL = [ ang_iR ang_iL]*180/pi
  qf = [ qf ; ...
         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  -ang_r  ang_iL  q9  q10  -q9-q10  -ang_iL  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 6) Baixar a perna esquerda ao longo do seu novo plano: ang_iL
%    -> confirmar que as Xf,Yf coincidem com as coordenadas X14,Y14
%-------------------------------------------------------------------------------------- 
  [q9,q10] = CinInv2R( [L_aux/sin(ang_iL )-L(2)-L(5) 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
  qf = [ qf ; ...
         ang_iR  q2  q3  -q2-q3  -ang_iR  0  ,  -ang_r  ang_iL  q9  q10  -q9-q10  -ang_iL  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 7) Alinhar a secçao da anca com os pes 
%--------------------------------------------------------------------------------------  
  ang_t = atan2( L_aux*sin(ang_r),D1+L_aux*cos(ang_r) );
  dL = L_aux/sin(ang_iL )-Hh/cos( ang_iR );
  [q9,q10] = CinInv2R( [Hh/cos( ang_iR )-L(2)-L(5) 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
 % qf = [ qf ; ...
 %        0  q2  q3  -q2-q3  0  -ang_t  ,  -ang_r+ang_t  0  q9  q10  -q9-q10  0  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  
% 8) Transferir o CoGx,y para o pe esquerdo: movimento lateral novo
% ATENÇAO: sera possivel juntar os passos 7 e 8 num unico passo!
%--------------------------------------------------------------------------------------  
  ang_iR = atan2(D1,Hh);    Lt_R = D1/sin( ang_iR );
  [q2,q3] = CinInv2R( [Lt_R-L(2)-L(5) 0],[L(3) L(4)],[-1 1] ); q2 = -q2; q3 = -q3;
  ang_iL = atan2(L(8)-L_aux,Hh);    Lt_L = (L(8)-L_aux)/sin( ang_iL ); 
  [q9,q10] = CinInv2R( [Lt_L-L(2)-L(5) 0],[L(4) L(3)],[-1 1] ); q9 = -q9; q10 = -q10;
  qf = [ qf ; ...
         -ang_iR  q2  q3  -q2-q3  ang_iR  -ang_r  ,  0  -ang_iL  q9  q10  -q9-q10  ang_iL  ,  0 0 0 0 0 0 0 0 ];
%--------------------------------------------------------------------------------------  

%--------------------------------------------------------------------------------------  
% Tarefa a Executar: "Rodar-Sobre-Si-Proprio"
%--------------------------------------------------------------------------------------  
  %------------------------------------------------------------------------------------
  % Planeamento de trajectorias nas juntas (funçoes polinomiais)
  %------------------------------------------------------------------------------------
   Texe = 1.0; Ts = Texe/25; %-- Periodo de amostragem e tempo de execuçao ------------
   tp = 0:Ts:Texe;           %-- Vector tempo virtual ---------------------------------
   Tsim = 0;                 %-- Tempo de simulaçao (inicializaçao) -------------------
   for k=1:size(qf,1),       %-- Movimento dividido em "i" fases --------------------#1 
      
   %__ Matriz tempo de simulaçao ------------------------------------------------------
    t(k,:) = tp+Tsim; Tsim = Tsim+Texe;
   %__ Velocidade e acelaraçao inicial e final ----------------------------------------
    for n=1:length(qi), VEL(n,:) = [0 0]; ACE(n,:) = [0 0]; end 
    [Q_legs,dQ_legs,ddQ_legs] = PlaneamentoJuntas( tp,qi,qf(k,:),VEL,ACE );     
   %__ Vector das juntas --------------------------------------------------------------
    Q(:,:,k) = Q_legs;
   %__ Reinicializaçao do vector das juntas -------------------------------------------
    qi = qf(k,:);   
        
   end %-----------------------------------------------------------------------------#1
  %------------------------------------------------------------------------------------
  %-- Coordenadas Cartesianas do robo completo
  %------------------------------------------------------------------------------------
   for i=1:size(Q,3), %-- Movimento dividido em "i" fases -----------------------------
   for k=1:size(Q,2), %-- ... ao longo do tempo "k" -----------------------------------
    
    %-- Posiçoes angulares em funçao do tempo -----------------------------------------
     Q_t = [ Q(1,k,i)   Q(2,k,i)   Q(3,k,i)   Q(4,k,i)    Q(5,k,i)   Q(6,k,i) ...
             Q(7,k,i)   Q(8,k,i)   Q(9,k,i)   Q(10,k,i)   Q(11,k,i)  Q(12,k,i)... 
             Q(13,k,i)  Q(14,k,i)  Q(15,k,i)  Q(16,k,i) ...   
             Q(17,k,i)  Q(18,k,i) Q(19,k,i)  Q(20,k,i) ];  
    %-- Coordenadas Cartesianas do sistema --------------------------------------------      
     Ai = CinDir20dof( Q_t,L,XYZRef );   %-- SupLeg = 1 -> RL
     [P,C,F,H,W,E] = CoordJuntas( Ai,XYZRef,L,O ); %size(P), size(C), size(F), pause  
    %-- Coordenadas XYZ da origem dos sistemas de coordenadas de DH -------------------  
     P_x(:,k,i) = P(:,1);    P_y(:,k,i) = P(:,2);    P_z(:,k,i) = P(:,3);
    %-- Coordenadas dos cilindros (juntas rotacionais) --------------------------------  
     C_x1(:,:,i,k) = C(1,:,:);   C_y1(:,:,i,k) = C(3,:,:);   C_z1(:,:,i,k) = C(5,:,:);
     C_x2(:,:,i,k) = C(2,:,:);   C_y2(:,:,i,k) = C(4,:,:);   C_z2(:,:,i,k) = C(6,:,:);
    %-- Coordenadas dos pes -----------------------------------------------------------  
     F_x(:,:,i,k) = F(1,:,:);   F_y(:,:,i,k) = F(2,:,:);   F_z(:,:,i,k) = F(3,:,:);
    %-- Coordenadas da secçao da anca -------------------------------------------------  
     H_x(:,:,i,k) = H(1,:,:);   H_y(:,:,i,k) = H(2,:,:);   H_z(:,:,i,k) = H(3,:,:);
    %-- Coordenadas dos pes (pe livre) ------------------------------------------------  
     W_x(:,:,i,k) = W(1,:,:);   W_y(:,:,i,k) = W(2,:,:);   W_z(:,:,i,k) = W(3,:,:);     
    %-- Coordenadas dos pes (pe livre) ------------------------------------------------  
     E_x(:,:,i,k) = E(1,:,:);   E_y(:,:,i,k) = E(2,:,:);   E_z(:,:,i,k) = E(3,:,:);     
    %-- Rasto da trajectoria da anca e pe livre ---------------------------------------
      Rasto_x(:,k,i) = [P(9,1) ; P(18,1)];
      Rasto_y(:,k,i) = [P(9,2) ; P(18,2)];
      Rasto_z(:,k,i) = [P(9,3) ; P(18,3)];
    %-- Coordenadas do centro de massa ------------------------------------------------ 
     [aux_COG,COG_mi] = RobCOG( Ai,Q_t,COG_0,M );
     COG(:,k,i) = aux_COG;

   end
   end
  %size(P_x), size(C_x1), size(F_x), pause
  %------------------------------------------------------------------------------------

  %------------------------------------------------------------------------------------
  % Evolução temporal das juntas ao longo das varias fases
  %------------------------------------------------------------------------------------
  % PlotJuntas( t,Q )
  % PlotCCarts( t,P_x,P_y,P_z )
  % PlotCOG( t,COG )
   H_fig = figure(1);
   ButtonName = questdlg( 'Press a button...','','ChangeView','Animation ','Exit      ','Animation ');
   if ButtonName == 'Exit      ', close all, return, end
   if ButtonName == 'ChangeView', rotate3d(1), waitfor(H_fig,'WindowButtonDownFcn'), end
  %------------------------------------------------------------------------------------

  %------------------------------------------------------------------------------------
  % Animaçao do movimento
  %------------------------------------------------------------------------------------
  ButtonRepeat = 'Yes';
  while ButtonRepeat == 'Yes', %-- Ciclo repetitivo -----------------------------end#3
  
   for i=1:size(Q,3), %-- Movimento dividido em "i" fases -----------------------------
   for k=1:size(Q,2), %-- ... ao longo do tempo ---------------------------------------

       DesenharRobo( P_x(:,k,i), P_y(:,k,i), P_z(:,k,i),...
                     C_x1(:,:,i,k), C_y1(:,:,i,k), C_z1(:,:,i,k),...
                     C_x2(:,:,i,k), C_y2(:,:,i,k), C_z2(:,:,i,k),...
                     F_x(:,:,i,k), F_y(:,:,i,k), F_z(:,:,i,k),...
                     H_x(:,:,i,k), H_y(:,:,i,k), H_z(:,:,i,k),...
                     W_x(:,:,i,k), W_y(:,:,i,k), W_z(:,:,i,k),...
                     E_x(:,:,i,k), E_y(:,:,i,k), E_z(:,:,i,k),...                     
                     Rasto_x(:,k,i), Rasto_y(:,k,i), Rasto_z(:,k,i),...
                     XYZRef, COG(:,k,i) )                 
   end
             P1  = [ P_x(1,size(Q,2),i), P_y(1,size(Q,2),i), P_z(1,size(Q,2),i) ]
             P7 = [ P_x(7,size(Q,2),i), P_y(7,size(Q,2),i), P_z(7,size(Q,2),i) ]
             P8 = [ P_x(8,size(Q,2),i), P_y(8,size(Q,2),i), P_z(8,size(Q,2),i) ]
             P9 = [ P_x(9,size(Q,2),i), P_y(9,size(Q,2),i), P_z(9,size(Q,2),i) ]
             P13 = [ P_x(13,size(Q,2),i), P_y(13,size(Q,2),i), P_z(13,size(Q,2),i) ]
             P14 = [ P_x(14,size(Q,2),i), P_y(14,size(Q,2),i), P_z(14,size(Q,2),i), max(abs( P_z(14,:,i) )) ]
             Lt_RL  = [P_z(6,size(Q,2),i)-P_z(2,size(Q,2),i) P_z(9,size(Q,2),i)-P_z(13,size(Q,2),i)]
             
             pause

   end  
   
   ButtonRepeat = questdlg( 'Would you like to repeat?','','Yes','No ','Yes');           
   if ButtonRepeat == 'Yes',
      ButtonView = questdlg( 'Would you like to change view?','','Yes','No ','No ');
   if ButtonView == 'Yes', rotate3d(1), waitfor(H_fig,'WindowButtonDownFcn'), end
   end
   
  end
%--------------------------------------------------------------------------------------
close all, return
