restart:

#Método de lagrange

with(VectorCalculus):

with(LinearAlgebra):

#Origem

Orig:= <0|0|0>:

#Cinematica direta

T43:= Matrix([[1,0,0,0],[0,1,0,0],[0,0,1,L1],[0,0,0,1]]):

T76:= Matrix([[1,0,0,0],[0,1,0,0],[0,0,1,L2],[0,0,0,1]]):

R10:= Matrix([ [1,0,0,0] , [0,cos(q1(t)),-sin(q1(t)),0] , [0,sin(q1(t)),cos(q1(t)),0] , [0,0,0,1] ]):

R21:= Matrix([ [cos(q2(t)),0,-sin(q2(t)),0] , [0,1,0,0] , [sin(q2(t)),0,cos(q2(t)),0] , [0,0,0,1] ]):

R32:= Matrix([ [cos(q3(t)),sin(q3(t)),0,0] , [-sin(q3(t)),cos(q3(t)),0,0] , [0,0,1,0] , [0,0,0,1] ]):

R54:= Matrix([ [cos(q4(t)),0,-sin(q4(t)),0] , [0,1,0,0] , [sin(q4(t)),0,cos(q4(t)),0] , [0,0,0,1] ]):

R65:= Matrix([ [cos(q5(t)),sin(q5(t)),0,0] , [-sin(q5(t)),cos(q5(t)),0,0] , [0,0,1,0] , [0,0,0,1] ]):

Rr10:= Matrix([ [1,0,0] , [0,cos(q1(t)),-sin(q1(t))] , [0,sin(q1(t)),cos(q1(t))] ]):

Rr21:= Matrix([ [cos(q2(t)),0,-sin(q2(t))] , [0,1,0] , [sin(q2(t)),0,cos(q2(t))] ]):

Rr32:= Matrix([ [cos(q3(t)),sin(q3(t)),0] , [-sin(q3(t)),cos(q3(t)),0] , [0,0,1] ]):

Rr54:= Matrix([ [cos(q4(t)),0,-sin(q4(t))] , [0,1,0] , [sin(q4(t)),0,cos(q4(t))] ]):

Rr65:= Matrix([ [cos(q5(t)),sin(q5(t)),0] , [-sin(q5(t)),cos(q5(t)),0] , [0,0,1] ]):

#Coordenadas das juntas

A:= <0|0|0>:

Br:= R10.R21.R32.T43:

B:= <Br[1,4]|Br[2,4]|Br[3,4]>:

Cr:= R10.R21.R32.T43.R54.R65.T76:

C:= <Cr[1,4]|Cr[2,4]|Cr[3,4]>:

#Coordenadas dos centros de massa

TC43:= Matrix([[1,0,0,0],[0,1,0,0],[0,0,1,L1/2],[0,0,0,1]]):

MC1:=R10.R21.R32.TC43:

C1:=<MC1[1,4]|MC1[2,4]|MC1[3,4]>:

C1z := C1[3]:

TC76:= Matrix([[1,0,0,0],[0,1,0,0],[0,0,1,L2/2],[0,0,0,1]]):

MC2:=R10.R21.R32.T43.R54.R65.TC76:

C2:=<MC2[1,4]|MC2[2,4]|MC2[3,4]>:

C2z := C2[3]:

#Calculo da velocidade dos centros de massa

VPc1:= diff(C1,t):

VPc2:= diff(C2,t):

#Calculo da velocidade angular

wC1 := Transpose(Rr10.<v1,0,0> + Rr10.Rr21.<0,v2,0> + Rr10.Rr21.Rr32.<0,0,v3>):

wC2 := Transpose(Rr10.<v1,0,0> + Rr10.Rr21.<0,v2,0> + Rr10.Rr21.Rr32.<0,0,v3> + Rr10.Rr21.Rr32.Rr54.<0,v4,0>):

#Momento de inercia

Ic1:= (1/12)*m1*L1^2:

Ic2:= (1/12)*m2*L2^2:

#Energia cinética

Ec11:= (m1/2)*(VPc1.Transpose(VPc1)) + (Ic1/2)*(wC1.Transpose(wC1)):

Ec1:= simplify(Ec11):

Ec22:= (m2/2)*(VPc2.Transpose(VPc2)) + (Ic2/2)*(wC2.Transpose(wC2)):

Ec2:= simplify(Ec22):

#Energia potencial

Uc1:=m1.g.C1z:

Uc2:=m2.g.C2z:

#Energia cinetica - energia potencial

T1 := Ec1 + Ec2 - Uc1 - Uc2:

#T:= subs(diff(q1(t),t)=v1(t),diff(q2(t),t)=v2(t),diff(q3(t),t)=v3(t),diff(q4(t),t)=v4(t),diff(q5(t),t)=v5(t),diff(v1(t),t)=a1(t),diff(v2(t),t)=a2(t),diff(v3(t),t)=a3(t),diff(v4(t),t)=a4(t),diff(v5(t),t)=a5(t), T1):

T:= subs(diff(q1(t),t)=v1,diff(q2(t),t)=v2,diff(q3(t),t)=v3,diff(q4(t),t)=v4,diff(q5(t),t)=v5,q1(t)=q1,q2(t)=q2,q3(t)=q3,q4(t)=q4,q5(t)=q5, T1):

Eq11:=diff(T,v1):

#Tv1:=convert(Tv1,diff):

Eq12:=diff(T,q1):

#Tq1:=convert(Tq1,diff):

Eq13 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq11):

Eq14 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq12):

Eq15:= diff(Eq13,t):

Eqqqq16 := Eq15-Eq14=0:

##Lagrangiano

Eqqq16:=expand(Eqqqq16):

Eqq16:=convert(Eqqq16,diff):

Eq16:=collect(Eqq16,diff):

Eq21:=diff(T,v2):

#Tv1:=convert(Tv1,diff):

Eq22:=diff(T,q2):

#Tq1:=convert(Tq1,diff):

Eq23 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq21):

Eq24 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t),Eq22):

Eq25:= diff(Eq23,t):

Eqqqq26 := Eq25-Eq24=0:

##Lagrangiano

Eqqq26:=expand(Eqqqq26):

Eqq26:=convert(Eqqq26,diff):

Eq26:=collect(Eqq26,diff):

Eq31:=diff(T,v3):

#Tv1:=convert(Tv1,diff):

Eq32:=diff(T,q3):

#Tq1:=convert(Tq1,diff):

Eq33 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq31):

Eq34 := subs(vq1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq32):

Eq35:= diff(Eq33,t):

Eqqqq36 := Eq35-Eq34=0:

##Lagrangiano

Eqqq36:=expand(Eqqqq36):

Eqq36:=convert(Eqqq36,diff):

Eq36:=collect(Eqq36,diff):

Eq41:=diff(T,v4):

#Tv1:=convert(Tv1,diff):

Eq42:=diff(T,q4):

#Tq1:=convert(Tq1,diff):

Eq43 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq41):

Eq44 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq42):

Eq45:= diff(Eq43,t):

Eqqqq46 := Eq45-Eq44=0:

##Lagrangiano

Eqqq46:=expand(Eqqqq46):

Eqq46:=convert(Eqqq46,diff):

Eq46:=collect(Eqq46,diff):

Eq51:=diff(T,v5):

#Tv1:=convert(Tv1,diff):

Eq52:=diff(T,q5):

#Tq1:=convert(Tq1,diff):

Eq53 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq15):

Eq54 := subs(q1=q1(t),q2=q2(t),q3=q3(t),q4=q4(t),q5=q5(t),v1=diff(q1(t),t),v2=diff(q2(t),t),v3=diff(q3(t),t),v4=diff(q4(t),t),v5=diff(q5(t),t), Eq52):

Eq55:= diff(Eq53,t):

Eqqqq56 := Eq55-Eq54=0:

##Lagrangiano

Eqqq56:=expand(Eqqqq56):

Eqq56:=convert(Eqqq56,diff):

Eq56:=collect(Eqq56,diff):

##Substituicao de dados

Lagran1 := subs[eval](L1=1, m1=1, L2=1, m2=1, g=9.81 , Eq16):

Lagran2 := subs[eval](L1=1, m1=1, L2=1, m2=1, g=9.81 , Eq26):

Lagran3 := subs[eval](L1=1, m1=1, L2=1, m2=1, g=9.81 , Eq36):

Lagran4 := subs[eval](L1=1, m1=1, L2=1, m2=1, g=9.81 , Eq46):

Lagran5 := subs[eval](L1=1, m1=1, L2=1, m2=1, g=9.81 , Eq56):

## Solucao do sistema para encontrar as derivadas segundas ddqn/dt

ini:= q1(0)= Pi/10, q2(0)=0, q3(0)=0, q4(0)=0, q5(0)=0, eval (diff (q1(t), t), t=0)=0,eval (diff (q2(t), t), t=0)=0, eval (diff (q3(t), t), t=0)=0, eval (diff (q4(t), t), t=0)=0, eval (diff (q5(t), t), t=0)=0,eval (diff (q1(t), t$2), t=0)=0,eval (diff (q2(t), t$2), t=0)=0, eval (diff (q3(t), t$2), t=0)=0, eval (diff (q4(t), t$2), t=0)=0, eval (diff (q5(t), t$2), t=0)=0:

sol := dsolve({Lagran1, Lagran2, Lagran3, Lagran4, Lagran5, ini},{q1(t), q2(t), q3(t), q4(t), q5(t)}, numeric, output=listprocedure):