function [P,C,F,E] = CoordJuntas( Ai,XYZRef,L,O ) %---------------------------------------------------------------------------------- %-- Coordenadas dos elos mecanicos: endoesqueleto formado por linhas %---------------------------------------------------------------------------------- T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; %-- DE perna direita ATE esquerda j = 1; P(j,:) = [XYZRef(1) XYZRef(2) XYZRef(3)]; j=j+1; %-- Referencia: P[1] = (X1,Y1,Z1) T_ind = 1:5; %-- Indices Ai: A1*A2*...A14 for i=1:length(T_ind); T = T*Ai(:,:,T_ind(i)); P(j,:) = T(1:3,4)'; j = j+1; end %---------------------------------------------------------------------------------- %-- Coordenadas do pe de suporte: 2 faces paralelas %---------------------------------------------------------------------------------- lf_l = O(1,1); lf_w = O(1,2); lf_h = O(1,3); T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; %-- Matriz identidade T = [1 0 0 XYZRef(1) ; 0 1 0 XYZRef(2) ; 0 0 1 XYZRef(3) ; 0 0 0 1]; %-- Pe de suporte F1 = [ -lf_w/2 lf_w/2 lf_w/2 -lf_w/2 %-- Top XY -lf_l/2 -lf_l/2 lf_l/2 lf_l/2 0 0 0 0 1 1 1 1 ]; F1 = T*F1; F2 = [ -lf_w/2 lf_w/2 lf_w/2 -lf_w/2 %-- Top XY -lf_l/2 -lf_l/2 lf_l/2 lf_l/2 0.01 0.01 0.01 0.01 1 1 1 1 ]; F2 = T*F2; F(1:3,:,3) = F1(1:3,:); F(1:3,:,4) = F2(1:3,:); %-- Pe de suporte %---------------------------------------------------------------------------------- %-- Coordenadas do exoesqueleto (perna): 2 faces %---------------------------------------------------------------------------------- ll_l = O(2,1); ll_w = O(2,2); ll_h = L(3); %-- Elo tornozelo-joelho T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; %-- Matriz identidade T_ind = 1:3; %-- Indices Ai: A1*A2*...A14 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end E1 = [ 0 0 -ll_h -ll_h %-- XX ll_w/2 -ll_w/2 -ll_w/2 ll_w/2 %-- YY ll_l/2 ll_l/2 ll_l/2 ll_l/2 %-- ZZ 1 1 1 1 ]; E1 = T*E1; E(1:3,:,1) = E1(1:3,:); E2 = [ 0 0 -ll_h -ll_h %-- XX -ll_w/2 ll_w/2 ll_w/2 -ll_w/2 %-- YY -ll_l/2 -ll_l/2 -ll_l/2 -ll_l/2 %-- ZZ 1 1 1 1 ]; E2 = T*E2; E(1:3,:,2) = E2(1:3,:); ll_h = L(4); %-- Elo joelho-anca T_ind = 4; %-- Indices Ai: A1*A2*...A14 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end E3 = [ 0 0 -ll_h -ll_h %-- XX ll_w/2 -ll_w/2 -ll_w/2 ll_w/2 %-- YY ll_l/2 ll_l/2 ll_l/2 ll_l/2 %-- ZZ 1 1 1 1 ]; E3 = T*E3; E(1:3,:,3) = E3(1:3,:); E4 = [ 0 0 -ll_h -ll_h %-- XX -ll_w/2 ll_w/2 ll_w/2 -ll_w/2 %-- YY -ll_l/2 -ll_l/2 -ll_l/2 -ll_l/2 %-- ZZ 1 1 1 1 ]; E4 = T*E4; E(1:3,:,4) = E4(1:3,:); ll_h = L(5); %-- Elo anca-anca T_ind = 5; %-- Indices Ai: A1*A2*...A14 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end E5 = [ 0 0 -ll_h -ll_h %-- XX ll_w/4 -ll_w/4 -ll_w/4 ll_w/4 %-- YY ll_l/2 ll_l/2 ll_l/2 ll_l/2 %-- ZZ 1 1 1 1 ]; E5 = T*E5; E(1:3,:,5) = E5(1:3,:); E6 = [ 0 0 -ll_h -ll_h %-- XX -ll_w/4 ll_w/4 ll_w/4 -ll_w/4 %-- YY -ll_l/2 -ll_l/2 -ll_l/2 -ll_l/2 %-- ZZ 1 1 1 1 ]; E6 = T*E6; E(1:3,:,6) = E6(1:3,:); %---------------------------------------------------------------------------------- %-- Coordenadas dos cilindros: 22 juntas rotacionais %---------------------------------------------------------------------------------- %-- Definiçao do cilindro original ------------------------------------------------ [X,Y,Z] = cylinder(1,100); X = 1*X; Y = 1*Y; Z = 7*(Z-0.5); X1 = X(1,:); Y1 = Y(1,:); Z1 = Z(1,:); X2 = X(2,:); Y2 = Y(2,:); Z2 = Z(2,:); Pcyl = [X1;X2;Y1;Y2;Z1;Z2;ones(1,length(X1))]; T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; j = 1; T_ind = 1:4; %-- Indices Ai: A1*A2*...A14 for i=1:length(T_ind); T = T*Ai(:,:,T_ind(i)); Cj([1 3 5 7],:) = T*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))]; Cj([2 4 6 7],:) = T*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))]; C(:,:,j) = Cj; j=j+1; end %-------------------------------------------------------------------------------end