In[]:=
faiancfrn[s1_,s2_,s3_]:={{Norm[s1]-1},{Norm[s2]-1},{Norm[s3]-1},{Transpose[s1].s2}[[1,1]],{Transpose[s1].s3}[[1,1]],{Transpose[s2].s3}[[1,1]]}addT[s1_]:={Table[ToExpression[j],{j,Table[StringInsert[ToString[i,InputForm],"[t]",-1],{i,Flatten[s1]}]}]}//Transposely=0.1;lr=0.0064;rou=7000;g=9.8;nxy=33;lm=π*lr^2*ly*rou;jx=1/12*lm*(ly^2+3*lr^2);jy=1/2*lm*lr^2;jz=jx;ii={{-0.5,0.5,0.5},{0.5,-0.5,0.5},{0.5,0.5,-0.5}}.{{jx},{jy},{jz}};m=ConstantArray[0,{21,21}];m[[1,1]]=lm;m[[2,2]]=lm;m[[3,3]]=lm;m[[4,4]]=ii[[1,1]];m[[5,5]]=ii[[1,1]];m[[6,6]]=ii[[1,1]];m[[7,7]]=ii[[2,1]];m[[8,8]]=ii[[2,1]];m[[9,9]]=ii[[2,1]];m[[10,10]]=ii[[3,1]];m[[11,11]]=ii[[3,1]];m[[12,12]]=ii[[3,1]];r={{rx},{ry},{rz}};r=addT[r];sx={{sx1},{sx2},{sx3}};sx=addT[sx];sy={{sy1},{sy2},{sy3}};sy=addT[sy];sz={{sz1},{sz2},{sz3}};sz=addT[sz];fair=r-{{sy1[x]/Norm[sy]},{sy2[x]/Norm[sy]},{0}};fais=faiancfrn[sx,sy,sz];q[t]=Flatten[{Flatten[r],Flatten[sx],Flatten[sy],Flatten[sz]}];fai=Flatten[{Flatten[fair],Flatten[fais]}];faiq=D[fai,{q[t]}];m[[Range[13,21,1],Range[1,12,1]]]=faiq;m[[Range[1,12,1],Range[13,21,1]]]=Transpose[faiq];qd[t]={D[q[t],t]}//Transpose;faiqqd=faiq.qd[t];faiqqdq=D[Flatten[faiqqd],{q[t]}];gama=faiqqdq.qd[t];gf={0,lm*g,0,0,0,0,0,0,0,0,0,0};(*{Flatten[gf]}//Transpose*)GF=Flatten[{Flatten[gf],Flatten[gama]}];lam={la1[t],la2[t],la3[t],la4[t],la5[t],la6[t],la7[t],la8[t],la9[t]};qdd=D[D[q[t],t],t];mf=Flatten[m.Transpose[{Join[qdd,lam]}]];cpdae=Table[mf[[i]]==GF[[i]],{i,1,21,1}];q0=Flatten[Table[ToExpression[j,TraditionalForm],{j,Table[StringReplace[ToString[q[t][[i]]],"t"->"0"],{i,1,12,1}]}]];qd0=Flatten[Table[ToExpression[j,TraditionalForm],{j,Table[StringReplace[ToString[qd[t][[i]]],"t"->"0"],{i,1,12,1}]}]];qq0={ly*Sin[30Degree],ly*Cos[30Degree],0,Cos[30Degree],Sin[30Degree],0,Sin[30Degree],Cos[30Degree],0,0,0,1};qdd0=Flatten[Table[ToExpression[j,TraditionalForm],{j,Table[StringReplace[ToString[qdd[[i]]],"t"->"0"],{i,1,12,1}]}]];lam0=Table[ToExpression[j],{j,Table[StringReplace[ToString[lam[[i]]],"t"->"0"],{i,1,9,1}]}];cpinit=Join[Table[q0[[i]]==qq0[[i]],{i,1,12,1}],Table[qd0[[i]]==0,{i,1,12,1}],Table[lam0[[j]]==0,{j,1,9,1}],Table[qdd0[[k]]==0,{k,1,12,1}]]NDSolve[{cpdae,cpinit},{rx,ry,rz,sx1,sx2,sx3,sy1,sy2,sy3,sz1,sz2,sz3,la1,la2,la3,la4,la5,la6,la7,la8,la9},{t,0,10},Method{"IndexReduction"Automatic}]
Out[]=
rx[0]0.05,ry[0]0.0866025,rz[0]0,sx1[0],sx2[0],sx3[0]0,sy1[0],sy2[0],sy3[0]0,sz1[0]0,sz2[0]0,sz3[0]1,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,la1[0]0,la2[0]0,la3[0]0,la4[0]0,la5[0]0,la6[0]0,la7[0]0,la8[0]0,la9[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0,[0]0
3
2
1
2
1
2
3
2
′
rx
′
ry
′
rz
′
sx1
′
sx2
′
sx3
′
sy1
′
sy2
′
sy3
′
sz1
′
sz2
′
sz3
′′
rx
′′
ry
′′
rz
′′
sx1
′′
sx2
′′
sx3
′′
sy1
′′
sy2
′′
sy3
′′
sz1
′′
sz2
′′
sz3
Out[]=