%----------------------------------------------------------------------
%                             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
%----------------------------------------------------------------------
%
% Script DHPERNAS - Carrega os dados cinemáticos das pernas do robot 
%                   com base nas convenções de Denavit-Hartenberg.
%                   Esta matriz ainda nao contem o elo que representa o 
%                   tronco do robot.


% Comprimento dos elos:
%  L = [42, 160 , 120 , 55 , 150 , 200];
% L = [L1, L2 , L3 , L4 , L5 , L6];
 L = [40, 120 , 100 , 45 , 125 , 150];

  
% Parâmetros cinemáticos das duas pernas:


%       | Junta |  Teta |  Alfa  |   l   |  d   |
%       |-------|-------|--------|-------|------|
        
Duas_Pernas = [   1   ,   0   ,  pi/2 ,  L(1) ,  0  
                  1   ,   0   ,   0   ,  L(2) ,  0
                  1   ,   0   ,   0   ,  L(3) ,  0
                  1   ,   0   , -pi/2 ,  L(4) ,  0 
                  1   , -pi/2 ,   0   ,  L(5) ,  0     
                  1   ,  pi/2 ,  pi/2 , -L(4) ,  0 
                  1   ,   0   ,   0   , -L(3) ,  0 
                  1   ,   0   ,   0   , -L(2) ,  0  
                  1   ,   0   , -pi/2 , -L(1)+20 ,  0
                  1   ,   0   ,   0   ,   -20   ,  0 ];       
