-
Notifications
You must be signed in to change notification settings - Fork 0
/
Derivation_of_Equations.nb
41 lines (33 loc) · 1.17 KB
/
Derivation_of_Equations.nb
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
xy1[t]=-L1/2*Cos[th1[t]];
z1[t]=-L1/2*Sin[th1[t]];
xy2[t]=-(L1*Cos[th1[t]]+L2/2*Cos[th2[t]]);
z2[t]=-(L1*Sin[th1[t]]+L2/2*Sin[th2[t]]);
xy3[t]=-(L1*Cos[th1[t]]+L2*Cos[th2[t]]+L3/2*Cos[th3[t]]);
z3[t]=-(L1*Sin[th1[t]]+L2*Sin[th2[t]]+L3/2*Sin[th3[t]]);
x1[t]=xy1[t]*Cos[phi[t]];
y1[t]=xy1[t]*Sin[phi[t]];
x2[t]=xy2[t]*Cos[phi[t]];
y2[t]=xy2[t]*Sin[phi[t]];
x3[t]=xy3[t]*Cos[phi[t]];
y3[t]=xy3[t]*Sin[phi[t]];
r1[t]=D[z1[t],t]^2+D[x1[t],t]^2+D[y1[t],t]^2;
Simplify[r1[t]];
r2[t]=D[z2[t],t]^2+D[x2[t],t]^2+D[y2[t],t]^2;
Simplify[Expand[r2[t]]];
r3[t]=D[z3[t],t]^2+D[x3[t],t]^2+D[y3[t],t]^2;
Simplify[Expand[r3[t]]];
K1a=1/2*(m1*r1[t]+m2*r2[t]+m3*r3[t]);
T=1/2*(I1*(th1'[t]^2+Cos[th1[t]]^2*phi'[t]^2)+I2*(th2'[t]^2+Cos[th2[t]]^2*phi'[t]^2)+I3*(th3'[t]^2+Cos[th3[t]]^2*phi'[t]^2));
Kt=K1a+T//FullSimplify
Vspr=K1/2*(th1[t]-th1eq)^2+K2/2*(th2[t]-th2eq)^2+K3/2*(th3[t]-th3eq)^2+K4/2*(phi[t]-phieq)^2;
Vgrav=m1*g*z1[t]+m2*g*z2[t]+m3*g*z3[t];
V=Vspr+Vgrav;
L=Kt-V;
a=D[D[L,th1'[t]],t]-D[L,th1[t]]+b1*th1'[t];
a//FullSimplify
b=D[D[L,th2'[t]],t]-D[L,th2[t]]+b2*th2'[t];
b//FullSimplify
c=D[D[L,th3'[t]],t]-D[L,th3[t]]+b3*th3'[t];
c//FullSimplify
d=D[D[L,phi'[t]],t]-D[L,phi[t]]+b4*phi'[t];
d//FullSimplify