function [P,B,H,C] = RobCoord( Ai,L )

%-- Vector comprimentos: L = [l0 l1 l2 l3 l4 lt_L lt_W lt_H];
 l0 = L(1); l1 = L(2);
 lh_l = L(5); lh_w = L(4); lh_h = L(3);
 lt_l = L(6); lt_w = L(7); lt_h = L(8);

%----------------------------------------------------------------------------------
%-- Coordenadas dos elos mecanicos: endoesqueleto formado por linhas
%----------------------------------------------------------------------------------
 P = [ 0  0   0        0 
       0  0   0        0
       0  l0  l0+lt_h  l0+lt_h+l1 ];
  
%----------------------------------------------------------------------------------
%-- Coordenadas da secçao do tronco: 4 faces
%----------------------------------------------------------------------------------
 T = [ 0 1 0 0; 
       0 0 1 0; 
       1 0 0 l0; 
       0 0 0 1];
 B1 = [  lt_h     lt_h     lt_h    lt_h   %-- XX
         lt_w/2  -lt_w/2  -lt_w/2  lt_w/2 %-- YY
        -lt_l/2  -lt_l/2   lt_l/2  lt_l/2 %-- ZZ
         1       1       1         1      ]; B1 = T*B1;  

 B2 = [  0        0        0       0      %-- XX
         lt_w/2  -lt_w/2  -lt_w/2  lt_w/2 %-- YY
        -lt_l/2  -lt_l/2   lt_l/2  lt_l/2 %-- ZZ
         1       1       1         1      ]; B2 = T*B2;

 B3 = [  lt_h     lt_h     0        0      %-- XX
        -lt_w/2   lt_w/2   lt_w/2  -lt_w/2 %-- YY
        -lt_l/2  -lt_l/2  -lt_l/2  -lt_l/2 %-- ZZ
         1       1       1         1      ]; B3 = T*B3;                

 B4 = [  lt_h     lt_h     0        0      %-- XX
        -lt_w/2   lt_w/2   lt_w/2  -lt_w/2 %-- YY
         lt_l/2   lt_l/2   lt_l/2   lt_l/2 %-- ZZ
         1       1       1         1      ]; B4 = T*B4;  

 B(1:3,:,1) = B1(1:3,:); B(1:3,:,2) = B2(1:3,:); 
 B(1:3,:,3) = B3(1:3,:); B(1:3,:,4) = B4(1:3,:);

%----------------------------------------------------------------------------------
%-- Coordenadas da secçao da pan&tilt: 2 faces
%----------------------------------------------------------------------------------
 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];

 T = T*Ai(:,:,1)*Ai(:,:,2)*Ai(:,:,3);
 H1 = [  lh_w+0.007     lh_w+0.007     0       0      %-- XX
        -lh_l/2   lh_l/2   lh_l/2 -lh_l/2 %-- YY
         lh_h     lh_h     lh_h    lh_h   %-- ZZ
         1       1       1         1      ]; H1 = T*H1;  

 H2 = [  0        0        0       0      %-- XX
        -lh_l/2   lh_l/2   lh_l/2 -lh_l/2 %-- YY
         0        0        lh_h    lh_h   %-- ZZ
         1       1       1         1      ]; H2 = T*H2;

 H(1:3,:,1) = H1(1:3,:); H(1:3,:,2) = H2(1:3,:); 

%----------------------------------------------------------------------------------
%-- Coordenadas dos cilindros: 2 juntas rotacionais (Pan&Tilt)
%----------------------------------------------------------------------------------
 lf_l = 0.03; 
%-- Definiçao do cilindro original ------------------------------------------------
 [X,Y,Z] = cylinder(1,100); %-- Altura do cilindro: 0..1 --------------------------
 X = 0.007*X; Y = 0.007*Y; Z1 = lf_l*(Z-0.5); Z2 = 0.1*lf_l*(Z-0.5);
 Pcy1 = [X(1,:);X(2,:);Y(1,:);Y(2,:);Z1(1,:);Z1(2,:);ones(1,length(X(1,:)))];
 Pcy2 = [X(1,:);X(2,:);Y(1,:);Y(2,:);Z2(1,:);Z2(2,:);ones(1,length(X(1,:)))];
 
 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
 T = T*Ai(:,:,1);
 Trans = [ 1 0 0 0 ; 
           0 1 0 0 ; 
           0 0 1 -lf_l/4; 
           0 0 0 1 ]; %-- elevar a junta da anca
 Cj([1 3 5 7],:) = T*Trans*[Pcy1(1,:); Pcy1(3,:); Pcy1(5,:); ones(1,length(Pcy1(1,:)))];
 Cj([2 4 6 7],:) = T*Trans*[Pcy1(2,:); Pcy1(4,:); Pcy1(6,:); ones(1,length(Pcy1(1,:)))];
 C(:,:,1) = Cj;
 
 T = T*Ai(:,:,2);
 Cj([1 3 5 7],:) = T*[Pcy1(1,:); Pcy1(3,:); Pcy1(5,:); ones(1,length(Pcy1(1,:)))];
 Cj([2 4 6 7],:) = T*[Pcy1(2,:); Pcy1(4,:); Pcy1(6,:); ones(1,length(Pcy1(1,:)))];
 C(:,:,2) = Cj;
 
 T = T*Ai(:,:,3)*Ai(:,:,4)*Ai(:,:,5);
 Cj([1 3 5 7],:) = T*[Pcy2(1,:); Pcy2(3,:); Pcy2(5,:); ones(1,length(Pcy2(1,:)))];
 Cj([2 4 6 7],:) = T*[Pcy2(2,:); Pcy2(4,:); Pcy2(6,:); ones(1,length(Pcy2(1,:)))];
 C(:,:,3) = Cj;
%-------------------------------------------------------------------------------end
