>Restart():

>DeclareObservers(N,B,Spring):
>DeclarePoints(N,B,P,seq(seq(cat(E,i,j),i=1..3),j=1..4)):
>DeclareTriads(n,b,spring):
>DefineObservers([N,N,n],[B,B,b],[Spring,N,spring]):
>DefinePoints([N,B,n,q1,q2,q3],[B,P,b,p1,p2,p3],[B,E11,b,0,l2/2,l3/2],[B,E12,b,0,l2/2,-l3/2],[B,E13,b,0,-l2/2,-l3/2],[B,E14,b,0,-l2/2,l3/2],[B,E21,b,l1/2,0,l3/2],[B,E22,b,l1/2,0,-l3/2],[B,E23,b,-l1/2,0,-l3/2],[B,E24,b,-l1/2,0,l3/2],[B,E31,b,l1/2,l2/2,0],[B,E32,b,l1/2,-l2/2,0],[B,E33,b,-l1/2,-l2/2,0],[B,E34,b,-l1/2,l2/2,0]):
>DefineTriads([n,b,[q4,3],[q5,1],[q6,3]],[n,spring,[Matrix([[r11,r12,r13],[r21,r22,r23],[r31,r32,r33]])],[Matrix([[scaleon*le/2,0,0],[0,scaleon*le/2,0],[0,0,1]])]]):
>DefineNeighbors([N,B],[N,Spring]):
>assume(d1,real):assume(d2,real):assume(d3,real):
>d:=MakeTranslations(n,d1,d2,d3):
>t3:=(1/VectorLength(d)) &** d:
>t1:=(1/VectorLength(t3 &xx MakeTranslations(n,0,-((d1^2+d2^2)&==0),((d1^2+d2^2)&!=0)))) &** (t3 &xx MakeTranslations(n,0,-((d1^2+d2^2)&==0),((d1^2+d2^2)&!=0))):
>t2:=t3 &xx t1:
>rot:=[seq(seq(cat(r,i,j)=simplify(MakeTranslations(n,i) &oo cat(t,j)),i=1..3),j=1..3)]:

>DefineObjects(
   seq([B,'Block',point=cat(E,1,j),xlength=l1,ylength=(l1+l2+l3)/30,zlength=(l1+l2+l3)/30],j=1..4),
   seq([B,'Block',point=cat(E,2,j),xlength=(l1+l2+l3)/30,ylength=l2,zlength=(l1+l2+l3)/30],j=1..4),
   seq([B,'Block',point=cat(E,3,j),xlength=(l1+l2+l3)/30,ylength=(l1+l2+l3)/30,zlength=l3],j=1..4)):
>DefineObjects(
   [B,'Sphere',point=P,radius=le,color=blue],
   [N,'Sphere',radius=le,color=white]):
>r:=MakeTranslations(spring,cos(2*t),sin(2*t),l*t):
>scale:=(1/VectorLength(DiffTime(r,spring))):
>b3:= scale &** DiffTime(r,spring):
>kappa:=VectorLength(scale &** DiffTime(b3,spring)):
>b1:=(scale/kappa) &** DiffTime(b3,spring):
>b2:=b3 &xx b1:
>tau:=VectorLength(scale &** DiffTime(b2,spring)):
>localrot:=Matrix(3,3,(i,j)->MakeTranslations(spring,i) &oo cat(b,j)):
>DefineObjects(seq(evalf([Spring,'Cylinder',point=subs(t=(i-1)*0.2,eval(r)),orient=subs(t=(i-1)*0.2,eval(localrot)),radius=0.05,length=subs(t=(i-1)*0.2,0.2/scale),color=yellow]),i=1..150)):

>DeclareStates(q1,q2,q3,q4,q5,q6):
>kde:={seq(LinearVelocity(N,B) &oo MakeTranslations(n,i)=cat(u,i),i=1..3),seq(AngularVelocity(n,b) &oo MakeTranslations(b,i)=cat(u,i+3),i=1..3)}:
>kde:=solve(kde,{q1t,q2t,q3t,q4t,q5t,q6t}):
>DeclareStates(u1,u2,u3,u4,u5,u6):
>p:=subs(kde,LinearMomentum(N,B)):
>h:=subs(kde,AngularMomentum(N,b)):
>rNP:=FindTranslation(N,P):
>rBP:=FindTranslation(B,P):
>dde:={seq((DiffTime(p,n) &-- ((k*(l0/VectorLength(rNP)-1)) &** rNP) &-- MakeTranslations(n,0,0,-M*g)) &oo MakeTranslations(n,i)=0,i=1..3),seq((DiffTime(h,n) &-- (rBP &xx ((k*(l0/VectorLength(rNP)-1)) &** rNP))) &oo MakeTranslations(b,i)=0,i=1..3)}:

>GeometryOutput(main=N,states=[q1,q2,q3,q4,q5=0.5,q6,u1,u2,u3,u4,u5,u6],parameters=[l1=1,l2=.5,l3=.25,marker=.1,Inertia11=2,Inertia12,Inertia13,Inertia21,Inertia22=1,Inertia23,Inertia31,Inertia32,Inertia33=.5,M=2,g=10,k=10,l0=1,p1=0.5,p2=.25,p3=-.125],anims=[le=.1,d1=q1+(cos(q4)*cos(q6)-sin(q4)*cos(q5)*sin(q6))*p1+(-cos(q4)*sin(q6)-sin(q4)*cos(q5)*cos(q6))*p2+sin(q4)*sin(q5)*p3,d2=q2+(sin(q4)*cos(q6)+cos(q4)*cos(q5)*sin(q6))*p1+(-sin(q4)*sin(q6)+cos(q4)*cos(q5)*cos(q6))*p2-cos(q4)*sin(q5)*p3,d3=q3+sin(q5)*sin(q6)*p1+sin(q5)*cos(q6)*p2+cos(q5)*p3,l=sqrt(d1^2+d2^2+d3^2)/29.8,codegen:-optimize(rot),scaleon=(k&!=0)],checktree,checkargs,filename="dynamics.geo"):
>MotionOutput(ode=kde union dde,states=[q1,q2,q3,q4,q5=0.5,q6,u1,u2,u3,u4,u5,u6],parameters=[l1=1,l2=.5,l3=.25,marker=.1,Inertia11=2,Inertia12,Inertia13,Inertia21,Inertia22=1,Inertia23,Inertia31,Inertia32,Inertia33=.5,M=2,g=10,k=10,l0=1,p1=0.5,p2=.25,p3=-.125],anims=[le=.1,d1=q1+(cos(q4)*cos(q6)-sin(q4)*cos(q5)*sin(q6))*p1+(-cos(q4)*sin(q6)-sin(q4)*cos(q5)*cos(q6))*p2+sin(q4)*sin(q5)*p3,d2=q2+(sin(q4)*cos(q6)+cos(q4)*cos(q5)*sin(q6))*p1+(-sin(q4)*sin(q6)+cos(q4)*cos(q5)*cos(q6))*p2-cos(q4)*sin(q5)*p3,d3=q3+sin(q5)*sin(q6)*p1+sin(q5)*cos(q6)*p2+cos(q5)*p3,l=sqrt(d1^2+d2^2+d3^2)/29.8,codegen:-optimize(rot),scaleon=(k&!=0)],checksings,checkargs,filename="dynamics.dyn");