%--------------------------------------------------------------------------------------
% Ficheiro:		SimWalking.m
%
% Autor: 		Filipe Silva
% Data:			31Maio2004
% Objectivo:	Anumação cinemática do movimento do robo bipede 12dof-3D
%--------------------------------------------------------------------------------------

clear all
%clc
%warning off

%--------------------------------------------------------------------------------------
% Definição de CONSTANTES & OPÇOES
%--------------------------------------------------------------------------------------
%-- Estrutura do braço manipulador ----------------------------------------------------
 l0 =0.02;	  %-- Comprimento do elo pe-tornozelo -----------
 l1 =0.022;	  %-- Comprimento do elo tornozelo-tornozelo ----
 l2 =0.16;	  %-- Comprimento do elo tornozelo-joelho -------
 l3 =0.12;    %-- Comprimento do elo joelho-anca ------------
 l4 =0.053;   %-- Comprimento do elo anca-anca --------------
 l5 =0.018;   %-- Comprimento da carga colocada no tronco ---
 l5h=0.050;   %-- Comprimento/altura da secçao da anca ------
 l5t=0.125;   %-- Comprimento do elo anca-tronco ------------
 l6 =0.15;    %-- Comprimento/largura da secçao da anca -----
 L=[l0 l1 l2 l3 l4 l5 l5h l5t l6]; 
%-- Definiçoes sobre o ciclo de locomoçao ---------------------------------------------
 DIM = 24;                      %-- Dimensão do vector tempo -------------------------- 
 SS = 1; DS = 0;                %-- Definiçao das fases de SS e DS --------------------
 RL = 1; LL = 0;                %-- Perna de referencia -------------------------------
 XYRef = [0 0];                 %-- Coordenadas de referencia -------------------------
%------------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Representação gráfica do robô na configuração inicial: POSIÇAO ZERO 
%		1) Função CinDir2R: cálculo das coordenadas Cartesianas do robô RR
%		2) Script DesenharRobo: plot dos elos mecânicos, juntas e músculos
%--------------------------------------------------------------------------------------
qi = [0 0 0 0 0 0 0 0 0 0 0 0];
Ai = CinDir12dof( qi,L,XYRef ); 
[P,C,F] = CoordJuntas( Ai,XYRef );

DesenharRoboPZ( P,C,F )

%-----------------------------------------------------------------------------------end

%--------------------------------------------------------------------------------------
% Ciclo de movimentos PtP: descrição dos requisitos da tarefa operacional
%--------------------------------------------------------------------------------------        
%-- Parametros de locomoçao -----------------------------------------------------------
 Sl = 0.15; Vf  = 0.1;          %-- Comprimento do passo e Velocidade de Progressao ---
 Hh = 0.39; Fc = 0.05;          %-- Altura da anca e Elevaçao do Pe -------------------
 COPy = 0.0;                    %-- 
%-- Especificaçao do movimento: vector P(xi,yi,zi) ------------------------------------
% Coxa =    xh      yh      zh      x18      y18      z18 = Pe Livre ------------------
FROM   = [  P(9,1)  P(9,2)  P(9,3)-l5h/2  P(18,1)  P(18,2)  P(18,3)  P(18,3) ];
MOVETO = [ -Sl/2    COPy    Hh   -Sl   NaN    0.0    0.0    SS
            Sl/2    COPy    Hh    Sl   NaN    0.0    Fc     SS
            Sl/2    0.075   Hh    Sl   NaN    0.0    0.0    DS ];

%-- Simulaçao do movimento ------------------------------------------------------------
for k=1:size(MOVETO,1),
   %-- Escolha dos movimentos e realizar ---------------------------------------------
    MOVE_OnOff = [ 1 1 1 ];
    if MOVE_OnOff(k) == 0, continue, end
   %-- Move k ------------------------------------------------------------------------
    Xhi = FROM(1);       Yhi = FROM(2);       Zhi = FROM(3);        %-- ... from   
    Xhf = MOVETO(k,1);   Yhf = MOVETO(k,2);   Zhf = MOVETO(k,3);    %-- ... to  
    Xfi = FROM(4);       Yfi = FROM(5);       Zfi = FROM(6);        %-- ... from   
    Xff = MOVETO(k,4);   Yff = MOVETO(k,5);   Zff = MOVETO(k,6);    %-- ... to  
    
   %-- Simulação temporal ------------------------------------------------------------
    if Xhf==Xhi & Yhf==Yhi & Zhf==Zhi,
       Tsim	= sqrt((Xff-Xfi)^2+(Zff-Zfi)^2)/Vf;              %-- Tempo de execução ---
    else
        Tsim = sqrt((Xhf-Xhi)^2+(Yhf-Yhi)^2+(Zhf-Zhi)^2)/Vf; %-- Tempo de execução ---
    end
    Ts      = Tsim/DIM;
    t		= 0:Ts:Tsim;	    %-- Vector tempo (virtual)         -------------------
      
   %-- PERNA DE SUPORTE --------------------------------------------------------------
   %-- Cinematica inversa ------------------------------------------------------------
    qf_sup = CinInvHip_Sup( MOVETO(k,1:3),L ); 
   %-- Planeamento de trajectorias: VEL e ACE inicial e final ------------------------
    for n=1:length(qf_sup), VEL(n,:) = [0 0]; end %-- Velocidade inicial e final 
    for n=1:length(qf_sup), ACE(n,:) = [0 0]; end %-- Aceleraçao inicial e final
    [Q_sup,dQ_sup,ddQ_sup] = PlaneamentoJuntas( t,qi(1:6),qf_sup,VEL,ACE );     
   %-- PERNA LIVRE -------------------------------------------------------------------   
   switch MOVETO(k,8),
    case DS, %-- Fases de duplo suporte singular -------------------------------------
       %-- Cinematica inversa --------------------------------------------------------    
        qf_liv = CinInvHip_Sup( [ -Sl/2 l6 Hh ],L );
        qf_liv(1) = -qf_liv(1); qf_liv(5) = -qf_liv(5);
        for n=1:length(qf_liv), AUX_qf(n) = qf_liv(length(qf_liv)+1-n); end, qf_liv = AUX_qf;
        qf = [ qf_sup qf_liv ]; 
       %-- Planeamento de trajectorias: VEL e ACE inicial e final --------------------
        for n=1:length(qf_liv), VEL(n,:) = [0 0]; end %-- Velocidade inicial e final
        for n=1:length(qf_liv), ACE(n,:) = [0 0]; end %-- Aceleraçao inicial e final
        [Q_liv,dQ_liv,ddQ_liv] = PlaneamentoJuntas( t,qi(7:12),qf_liv,VEL,ACE ); 
    case SS, %-- Fases de suporte singular -------------------------------------------
    if MOVETO(k,6) == MOVETO(k,7),
       %-- Cinematica inversa --------------------------------------------------------    
        qf_liv = CinInvHip_Liv( qf_sup(1),MOVETO(k,1:6),L );
        qf = [ qf_sup qf_liv ]; 
       %-- Planeamento de trajectorias: VEL e ACE inicial e final --------------------
        for n=1:length(qf_liv), VEL(n,:) = [0 0]; end %-- Velocidade inicial e final
        for n=1:length(qf_liv), ACE(n,:) = [0 0]; end %-- Aceleraçao inicial e final
        [Q_liv,dQ_liv,ddQ_liv] = PlaneamentoJuntas( t,qi(7:12),qf_liv,VEL,ACE ); 
    else
        %-- Posiçao intermedia -------------------------------------------------------
         t1 = 0:Ts:DIM/2*Ts;	Tsim1 = DIM/2*Ts;      %-- Vector Tempo: elevar pe
         t2 = DIM/2*Ts:Ts:Tsim; Tsim2 = Tsim-DIM/2*Ts; %-- Vector Tempo: baixar pe
        %-- Cinematica inversa -------------------------------------------------------        
         qf_liv1 = CinInvHip_Liv( qf_sup(1),[ PxyzHip( Q_sup(1:3,DIM/2),L ) (Xff+Xfi)/2 Yff MOVETO(k,7) ],L );
         qf_liv2 = CinInvHip_Liv( qf_sup(1),MOVETO(k,1:6),L );
         qf = [ qf_sup qf_liv2 ];
        %-- Planeamento de trajectorias: VEL e ACE inicial e final -------------------
         for n=1:length(qf_liv1), 
             Vm(n) = mean( ( qf_liv1(n)-qi(n+6) )/Tsim1+( qf_liv2(n)-qf_liv1(n) )/Tsim2 );
         end      
         for n=1:length(qf_liv1), VEL(n,:) = [0 Vm(n)]; end %-- Velocidade inicial e final 
         for n=1:length(qf_liv1), ACE(n,:) = [0 0];     end %-- Aceleraçao inicial e final
         [Q_liv1,dQ_liv1,ddQ_liv1] = PlaneamentoJuntas( t1,qi(7:12),qf_liv1,VEL,ACE ); 
         for n=1:length(qf_liv1), VEL(n,:) = [Vm(n) 0]; end %-- Velocidade inicial e final
         for n=1:length(qf_liv1), ACE(n,:) = [0     0]; end %-- Aceleraçao inicial e final         
         [Q_liv2,dQ_liv2,ddQ_liv2] = PlaneamentoJuntas( t2,qf_liv1, qf_liv2,VEL,ACE ); 
        %-- Trajectorias globais -----------------------------------------------------
           Q_liv = [   Q_liv1   Q_liv2(:,2:length(  Q_liv2))];
          dQ_liv = [  dQ_liv1  dQ_liv2(:,2:length( dQ_liv2))];
         ddQ_liv = [ ddQ_liv1 ddQ_liv2(:,2:length(ddQ_liv2))];
    end   
   end
   Q(:,:,k) = [ Q_sup ; Q_liv ]; %-- dQ = [ dQ_sup ; dQ_liv ]; ddQ = [ddQ_sup ; ddQ_liv ];
  %-- Reinicializaçao de variaveis ----------------------------------------------------
   FROM = MOVETO(k,:);  qi = qf;
end

%--------------------------------------------------------------------------------------
% Animaçao do andar bipede
%--------------------------------------------------------------------------------------
 Animacao3D( Q,L,XYRef,Sl,COPy )%, end

%--------------------------------------------------------------------------------------
% Sair
%--------------------------------------------------------------------------------------
 clc, disp('-- Press any key to exit -----------------------------------------'), pause
 pause, close all, return
