%Program written by Michael Gray, MIT Graduate Student, Nov 20, 2002 %Modified March 6th, 2003; April 4th, 2003; & May 31st, 2003 function [X,Mn,F,C,U,O] = Assembly(B,S,P,L) %B is the complete set of independent loops of the motion graph %S is the complete set of twists vectors for the dof of all mates %P is the set of paths between part pairs for underconstraint analysis %L is the set of mates for overconstraint analysis %These commands find the size of each matrix %lambda is the number of loops %f is the number of dof of all mates %p is the number of part pairs analyzed %m is the number of mates analyzed [lambda,f]=size(B); [s1,s2]=size(S); [p,p2]=size(P); [m,l2]=size(L); %This statement insures that the matrices are the right dimensions if lambda==l2 & f==s1 & s2==6 & p2==f & l2==lambda %These commands create Mn from B and S %i, j, and h are used throughout this code as "for" loop variables %These commands creates the Moc matrix from the Boc and Soc matrices for i=1:lambda for j=1:f for h=1:6 Mn((i-1)*6+h,j)=B(i,j)*S(j,h); end end end %This command creates the Mn Tranpose matrix Mnt=Mn'; %These command displays the Mn and Mn Transpose matrices on the screen disp('Network Motion Matrix='); Mn disp('Transpose of the Network Motion Matrix='); Mnt %This command finds the rank of the Mn matrix r=rank(Mn); %This set of commands displays the number of over- and under constraints F=f-r; disp('Number of Excess Underconstraints='); F C=6*lambda-r; disp('Number of Excess Overconstraints='); C %This statement checks for underconstraints in the assembly %If yes, then the relative motion for each pair of P parts is determined if F~=0 %x and y are variables used only to aid in the computation %Mnr is the row-reduced echelon form of Mn %Mnrn is an empty matrix that aids in computation x=1; y=r+1; Mnr=rref(Mn); Mnrn=zeros(6*lambda,f); %These commands rearrange the column order of Mnr %This insures that no inactive mates are chosen as independent %Ym is a vector that describes this transformation for i=1:f if x<=r if Mnr(x,i)==1 for j=1:6*lambda Mnrn(j,x)=Mnr(j,i); Ym(1,x)=i; end x=x+1; else for h=1:6*lambda Mnrn(h,y)=Mnr(h,i); Ym(1,y)=i; end y=y+1; end else for h=1:6*lambda Mnrn(h,y)=Mnr(h,i); Ym(1,y)=i; end y=y+1; end end %These commands create Mr of size (f x F) %Ym is used to determine the row order %M1, M2, and M3 are variables used to simplify the calculations M1=zeros(r,F); for i=1:F for j=1:r M1(j,i)= -Mnrn(j,r+i); end end M2=eye(F); M3=[M1;M2]; Mr=zeros(f,F); for i=1:F for j=1:f Mr(Ym(1,j),i)=M3(j,i); end end %These commands create a vector I of size (F x 1) %I indicates the dof chosen to have independent velocity magnitudes %I and Mr are displayed on the screen for h=1:F I(1,h)=Ym(1,(r+h)); end disp('DOF Chosen To Have Independent Velocity Magnitudes='); I disp('Reduced Motion Matrix='); Mr %These commands create Mm, Pm, and Sm %Mm is of size (p*F x p*f) %Pm is of size (p*f x p*f) %Sm is of size (p*f x 6) Mm=zeros(p*F,p*f); Pm=zeros(p*f,p*f); Sm=S; for i=0:(p-1) for j=1:F for h=1:f Mm(j+i*F,h+i*f)=Mr(h,j); end end end for i=0:(p-1) for j=1:f Pm(j+i*f,j+i*f)=P(i+1,j); end end for i=1:(p-1) Sm=[Sm;S]; end %These commands calculate and display U of size (p*F x 6) disp('The Underconstraint Matrix='); U=Mm*Pm*Sm %These commands display a submatrix of U for each of the "p" pairs %The submatrix is called Up and is of size (F x 7) %The first columns in Up indicates the independent dof %x, y, z, and II are used only to simplify the calculations for i=0:(p-1) Up=0; II=0; x=0; y=0; for j=1:F z=0; for h=1:6 z=z+abs(U(j+i*F,h)); end x=x+z; if z==0 else y=y+1; for h=1:6 Up(y,h)=U(j+i*F,h); end II(y,1)=I(1,j); end end if x==0 Part_Pair_Number=i+1 disp('There is no Relative Motion Between This Pair of Parts'); else Part_Pair_Number=i+1 Up=[II Up]; Relative_Motion_Between_Parts=Up end end %If F==O, there are no overconstraints, this is displayed on the screen else disp('There Is No Relative Motion Between Any Of The Parts'); U=NaN; end %This statement checks for overconstraints in the assembly %If there are, then for each of the m mates, these are determined if C~=0 %x and y are variables used only to aid in the computation %Mntr is the row-reduced echelon form of Mn Transpose %Mntrn is an empty matrix that aids in computation x=1; y=r+1; Mntr=rref(Mnt); Mntrn=zeros(f,6*lambda); %These commands rearrange the column order of Mntr %This insures no inactive forces/moments are chosen as independent %Yn is a vector that describes this transformation for i=1:6*lambda if x<=r if Mntr(x,i)==1 for j=1:f Mntrn(j,x)=Mntr(j,i); Yn(1,x)=i; end x=x+1; else for h=1:f Mntrn(h,y)=Mntr(h,i); Yn(1,y)=i; end y=y+1; end else for h=1:f Mntrn(h,y)=Mntr(h,i); Yn(1,y)=i; end y=y+1; end end %These commands create Mtr of size (6*lambda x C) %Yn is used to determine the row order %Mt1, Mt2, and Mt3 are variables used to simplify the calculations Mt1=zeros(r,C); for i=1:C for j=1:r Mt1(j,i)= -Mntrn(j,r+i); end end Mt2=eye(C); Mt3=[Mt1;Mt2]; Mtr=zeros(6*lambda,C); for i=1:C for j=1:6*lambda Mtr(Yn(1,j),i)=Mt3(j,i); end end %These commands create a vector J of size (C x 1) %J indicates the dof chosen to have independent velocity magnitudes for i=1:C J(1,i)=Yn(1,(r+i)); end %These commands rearrange J so its entries are in x,y,z,Mx,My,Mz form for i=1:lambda for j=1:C h=(J(1,j)-6*(i-1)); if h==1 J(1,j)=J(1,j)+3; elseif h==2 J(1,j)=J(1,j)+3; elseif h==3 J(1,j)=J(1,j)+3; elseif h==4 J(1,j)=J(1,j)-3; elseif h==5 J(1,j)=J(1,j)-3; elseif h==6 J(1,j)=J(1,j)-3; else end end end %These commands display the J vector and Mtr matrix disp('Loop Forces/Moments Chosen To Have Independent Magnitudes='); J disp('Reduced Network Motion Transpose Matrix='); Mtr %These commands create the Lm and Mtm matrices %Lm is of size (C*m x C*lambda) %Mtm is of size (C*m x 6) %D is a matrix of size (6 x 6) %D swaps the first and last three columns of a (nx6) matrix Lm=zeros(C*m,C*lambda); Mtm=zeros(C*lambda,6); D=[0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1; 1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0]; for i=1:m for j=1:C for h=1:lambda Lm((i-1)*C+j,h+(j-1)*lambda)=L(i,h); end end end for i=1:C for j=1:lambda for h=1:6 Mtm(j+(i-1)*lambda,h)=Mtr(h+(j-1)*6,i); end end end %%These commands calculate and display O of size (C*m x 6) disp('The Overconstraint Matrix='); O=Lm*Mtm*D %These commands display x submatrix of O for each of the n mates %The submatrix is called Om and is of size (C x 7) %The first columns of Op indicates the independent forces/moments %x, y, z, and JJ are used only to simplify the calculations for i=0:(m-1) Om=0; JJ=0; y=0; x=0; for j=1:C z=0; for h=1:6 z=z+abs(O(j+i*C,h)); end x=x+z; if z==0 else y=y+1; for h=1:6 Om(y,h)=O(j+i*C,h); end JJ(y,1)=J(1,j); end end if x==0 Mate_Number=i+1 disp('There Is No Overconstraint Transmitted By This Mate'); else Mate_Number=i+1 Om=[JJ Om]; Overconstraints_Through_Mate=Om end end %if C==0 then there aren'y any overconstraints and this is displayed else disp('There Are No Overconstraints In This Assembly'); O=NaN; end %A statement of the constraint status of the assembly is stored as X if F==0 & C==0 X=char('This Assembly Is Properly Constrained'); elseif F>0 & C==0 X=char('This Assembly Is Underconstrained'); elseif C>0 & F==0 X=char('This Assembly Is Overconstrained'); else X=char('This Assembly Is Both Over- And Under- Constrained'); end %This error message is stored as X if the input dimensions are not correct else X=char('Improper Input Matrix Dimensions'); Mn=NaN; F=NaN; C=NaN; U=NaN; O=NaN; end