In[146]:=
H* This is the Mathematica code for modelling a swinging door,
with a swinging cat door in it,
and a pendulum affixed to the cat door. Just like you see in any house. *L
In[147]:=
H* Load my package - this just gives me the "Unhat" command *L
In[148]:=
<< SMCS.m
Package "SMCS" defines: ACCSequations, ACCSsimulate, BodyAngularVelocity,
ConstrainedConnection, Force, GeneralizedCovariantDerivative, GetState, Hat, Hessian,
KErot, KEtrans, OrthogonalChristoffelSymbols, OrthogonalForce, OrthogonalProjection,
SetEqual, SMCSequations, SMCSsimulate, SpatialAngularVelocity, Unhat.
In[149]:=
H* Define coordinates as a function of time *L
In[150]:=
q = 8theta@tD, phi@tD, psi@tD<
Out[150]=
8theta@tD, phi@tD, psi@tD<
In[151]:=
H* Differentiate coordinates to get velocity *L
In[152]:=
vel = D@q, tD
Out[152]=
8theta£ @tD, phi£ @tD, psi£ @tD<
In[153]:=
H* Orientation of body 1 *L
In[154]:=
Out[154]=
R1 = 88Cos@theta@tDD, 0, Sin@theta@tDD<,
8Sin@theta@tDD, 0, - Cos@theta@tDD<, 80, 1, 0<<
88Cos@theta@tDD, 0, Sin@theta@tDD<, 8Sin@theta@tDD, 0, - Cos@theta@tDD<, 80, 1, 0<<
In[155]:=
H* Orientation of body 2 relative to body 1 *L
In[156]:=
R2p = 881, 0, 0<, 80, Cos@phi@tDD, Sin@phi@tDD<, 80, - Sin@phi@tDD, Cos@phi@tDD<<
Out[156]=
881, 0, 0<, 80, Cos@phi@tDD, Sin@phi@tDD<, 80, - Sin@phi@tDD, Cos@phi@tDD<<
In[157]:=
H* Orientation of body 2 *L
In[158]:=
R2 = R1.R2p
Out[158]=
88Cos@theta@tDD, - Sin@phi@tDD Sin@theta@tDD, Cos@phi@tDD Sin@theta@tDD<,
8Sin@theta@tDD, Cos@theta@tDD Sin@phi@tDD, - Cos@phi@tDD Cos@theta@tDD<,
80, Cos@phi@tDD, Sin@phi@tDD<<
In[159]:=
H* Orientation of body 3 relative to body 2 *L
In[160]:=
R3p = 88Sin@psi@tDD, Cos@psi@tDD, 0<, 8- Cos@psi@tDD, Sin@psi@tDD, 0<, 80, 0, 1<<
Out[160]=
88Sin@psi@tDD, Cos@psi@tDD, 0<, 8- Cos@psi@tDD, Sin@psi@tDD, 0<, 80, 0, 1<<
In[161]:=
H* Orientation of body 2 *L
In[162]:=
R3 = [email protected];
In[163]:=
H* Position of body 1 *L
2
example.nb
In[164]:=
Out[164]=
r1 = 8w ê 2 Cos@theta@tDD, w ê 2 Sin@theta@tDD, h<
:
1
2
w Cos@theta@tDD,
1
2
w Sin@theta@tDD, h>
In[165]:=
H* Position of cat door hinge centre *L
In[166]:=
r2p = 8w ê 2 Cos@theta@tDD, w ê 2 Sin@theta@tDD, l<
Out[166]=
:
1
2
w Cos@theta@tDD,
1
2
w Sin@theta@tDD, l>
In[167]:=
H* Position of body 2 relative to cat door hinge centre in body 2 coordinates *L
In[168]:=
r2pp = 80, - h2, 0<
Out[168]=
80, - h2, 0<
In[169]:=
H* Position of body 2 *L
In[170]:=
r2 = r2p + R1.R2p.r2pp
Out[170]=
:
1
2
w Cos@theta@tDD + h2 Sin@phi@tDD Sin@theta@tDD,
- h2 Cos@theta@tDD Sin@phi@tDD +
1
2
w Sin@theta@tDD, l - h2 Cos@phi@tDD>
In[171]:=
H* Position of body 3 pivot in body 2 coordinates *L
In[172]:=
r3p = 80, - h3, 0<
Out[172]=
80, - h3, 0<
In[173]:=
H* Position of body 2 in body 2 coordinates *L
In[174]:=
r3pp = l3 ê 2 8Sin@psi@tDD, - Cos@psi@tDD, 0<
Out[174]=
:
1
2
l3 Sin@psi@tDD, -
1
2
l3 Cos@psi@tDD, 0>
In[175]:=
H* Position of body 3 *L
In[176]:=
r3 = r2p + R1.R2p.Hr3p + r3ppL;
In[177]:=
H* Translational velocities *L
In[178]:=
v1 = D@r1, tD
Out[178]=
:-
1
2
w Sin@theta@tDD theta£ @tD,
In[179]:=
v2 = Simplify@D@r2, tDD;
In[180]:=
v3 = Simplify@D@r3, tDD;
In[181]:=
H* Body angular velocities *L
1
2
w Cos@theta@tDD theta£ @tD, 0>
example.nb
In[182]:=
Out[182]=
In[183]:=
Out[183]=
In[184]:=
Out[184]=
Omega1 = Simplify@Unhat@[email protected]@R1, tDDD
80, theta£ @tD, 0<
Omega2 = Simplify@Unhat@[email protected]@R2, tDDD
8- phi£ @tD, Cos@phi@tDD theta£ @tD, Sin@phi@tDD theta£ @tD<
Omega3 = Simplify@Unhat@[email protected]@R3, tDDD
8- Sin@psi@tDD phi£ @tD - Cos@phi@tDD Cos@psi@tDD theta£ @tD,
- Cos@psi@tDD phi£ @tD + Cos@phi@tDD Sin@psi@tDD theta£ @tD,
psi£ @tD + Sin@phi@tDD theta£ @tD<
In[185]:=
H* Spatial angular velocities *L
In[186]:=
omega1 = Simplify@Unhat@D@R1, tD.Transpose@R1DDD
Out[186]=
In[187]:=
Out[187]=
In[188]:=
Out[188]=
80, 0, theta£ @tD<
omega2 = Simplify@Unhat@D@R2, tD.Transpose@R2DDD
8- Cos@theta@tDD phi£ @tD, - Sin@theta@tDD phi£ @tD, theta£ @tD<
omega3 = Simplify@Unhat@D@R3, tD.Transpose@R3DDD
8- Cos@theta@tDD phi£ @tD + Cos@phi@tDD Sin@theta@tDD psi£ @tD,
- Sin@theta@tDD phi£ @tD - Cos@phi@tDD Cos@theta@tDD psi£ @tD,
Sin@phi@tDD psi£ @tD + theta£ @tD<
In[189]:=
H* Inertia tensor and rotational kinetic energy for body 1 *L
In[190]:=
I1 = 88I111, I112, I113<, 8I112, I122, I123<, 8I113, I123, I133<<
Out[190]=
In[191]:=
88I111, I112, I113<, 8I112, I122, I123<, 8I113, I123, I133<<
KErot1 = Simplify@H1 ê 2L HI1.Omega1L.Omega1D
1
Out[191]=
2
I122 theta£ @tD2
In[192]:=
H* Inertia tensor and rotational kinetic energy for body 2 *L
In[193]:=
I2 = 88I211, 0, 0<, 80, I222, 0<, 80, 0, I233<<
Out[193]=
In[194]:=
88I211, 0, 0<, 80, I222, 0<, 80, 0, I233<<
KErot2 = Simplify@H1 ê 2L HI2.Omega2L.Omega2D
1
Out[194]=
2
II211 phi£ @tD2 + II222 Cos@phi@tDD2 + I233 Sin@phi@tDD2 M theta£ @tD2 M
In[195]:=
H* Inertia tensor and rotational kinetic energy for body 3 *L
In[196]:=
I3 = 88I311, 0, 0<, 80, I322, 0<, 80, 0, I322<<
Out[196]=
88I311, 0, 0<, 80, I322, 0<, 80, 0, I322<<
3
4
example.nb
In[197]:=
KErot3 = Simplify@H1 ê 2L HI3.Omega3L.Omega3D
1
Out[197]=
2
II311 HSin@psi@tDD phi£ @tD + Cos@phi@tDD Cos@psi@tDD theta£ @tDL2 +
I322 Hpsi£ @tD + Sin@phi@tDD theta£ @tDL2 +
I322 HCos@psi@tDD phi£ @tD - Cos@phi@tDD Sin@psi@tDD theta£ @tDL2 M
In[198]:=
H* Translational kinetic energies *L
In[199]:=
KEtrans1 = Simplify@H1 ê 2L m1 Hv1.v1LD
1
Out[199]=
8
In[200]:=
m1 w2 theta£ @tD2
KEtrans2 = Simplify@H1 ê 2L m2 Hv2.v2LD
1
Out[200]=
8
m2 I4 h22 phi£ @tD2 - 4 h2 w Cos@phi@tDD phi£ @tD theta£ @tD +
I2 h22 + w2 - 2 h22 Cos@2 phi@tDDM theta£ @tD2 M
In[201]:=
KEtrans3 = Simplify@H1 ê 2L m3 Hv3.v3LD;
In[202]:=
H* Total kinetic energy *L
In[203]:=
KE = Simplify@KEtrans1 + KEtrans2 + KEtrans3 + KErot1 + KErot2 + KErot3D;
In[204]:=
H* Kinetic energy metric *L
In[205]:=
metric = Simplify@Table@D@KE, vel@@iDD, vel@@jDDD, 8i, 1, 3<, 8j, 1, 3<DD;
In[206]:=
H* Inverse of kinetic energy metric, i.e., "G^\sharp" *L
In[207]:=
metricinv = Inverse@metricD;
In[208]:=
H* Christoffel symbols *L
In[209]:=
In[210]:=
In[211]:=
Out[211]=
In[212]:=
Out[212]=
In[213]:=
Out[213]=
In[214]:=
Out[214]=
connection = H1 ê 2L Table@H1 ê 2L
Sum@metricinv@@i, lDD HD@metric@@j, lDD, q@@kDDD + D@metric@@k, lDD, q@@jDDD D@metric@@j, kDD, q@@lDDDL, 8l, 1, 3<D, 8i, 1, 3<, 8j, 1, 3<, 8k, 1, 3<D;
H* Newtonian torques. I am assuming a torque actuating each of the three
angular degrees of freedom. Each torque possible affects each of the three
bodies. So I define tau@@j,kDD as the effect of torque j on body k. *L
tau@1, 1D = T1 80, 0, 1<
80, 0, T1<
tau@1, 2D = 80, 0, 0<
80, 0, 0<
tau@1, 3D = 80, 0, 0<
80, 0, 0<
tau@2, 1D = 80, 0, 0<
80, 0, 0<
example.nb
In[215]:=
Out[215]=
In[216]:=
Out[216]=
In[217]:=
Out[217]=
In[218]:=
Out[218]=
In[219]:=
Out[219]=
In[220]:=
In[221]:=
Out[221]=
tau@2, 2D = T2 8R2@@1, 3DD, R2@@2, 3DD, R2@@3, 3DD<
8T2 Cos@phi@tDD Sin@theta@tDD, - T2 Cos@phi@tDD Cos@theta@tDD, T2 Sin@phi@tDD<
tau@2, 3D = - tau@2, 2D
8- T2 Cos@phi@tDD Sin@theta@tDD, T2 Cos@phi@tDD Cos@theta@tDD, - T2 Sin@phi@tDD<
tau@3, 1D = 80, 0, 0<
80, 0, 0<
tau@3, 2D = T3 8R3@@1, 3DD, R3@@2, 3DD, R3@@3, 3DD<
8T3 Cos@phi@tDD Sin@theta@tDD, - T3 Cos@phi@tDD Cos@theta@tDD, T3 Sin@phi@tDD<
tau@3, 3D = T3 8R3@@1, 3DD, R3@@2, 3DD, R3@@3, 3DD<
8T3 Cos@phi@tDD Sin@theta@tDD, - T3 Cos@phi@tDD Cos@theta@tDD, T3 Sin@phi@tDD<
H* Power *L
pow = Simplify@Sum@tau@j, 1D.omega1, 8j, 1, 3<D +
Sum@tau@j, 2D.omega2, 8j, 1, 3<D + Sum@tau@j, 3D.omega3, 8j, 1, 3<DD
H- T2 + T3L psi£ @tD + HT1 + 2 T3 Sin@phi@tDDL theta£ @tD
In[222]:=
H* Components of Lagrangian force as coefficients of power *L
In[223]:=
Ftheta = Coefficient@pow, vel@@1DDD
Out[223]=
In[224]:=
Out[224]=
In[225]:=
Out[225]=
In[226]:=
Out[226]=
In[227]:=
In[228]:=
T1 + 2 T3 Sin@phi@tDD
Ftheta = Coefficient@pow, vel@@1DDD
T1 + 2 T3 Sin@phi@tDD
Fpsi = Coefficient@pow, vel@@3DDD
- T2 + T3
F = 8Ftheta, Fphi, Fpsi<
8T1 + 2 T3 Sin@phi@tDD, Fphi, - T2 + T3<
H* Left-hand side of equations of motion *L
lhs = Simplify@Table@D@q@@iDD, 8t, 2<D +
Sum@connection@@i, j, kDD vel@@jDD vel@@kDD, 8j, 1, 3<, 8k, 1, 3<D, 8i, 1, 3<DD;
In[229]:=
H* Right-hand side of equations of motion *L
In[230]:=
rhs = Table@F = 8Ftheta, Fphi, Fpsi<, 8i, 1, 3<D
Out[230]=
88T1 + 2 T3 Sin@phi@tDD, Fphi, - T2 + T3<,
8T1 + 2 T3 Sin@phi@tDD, Fphi, - T2 + T3<, 8T1 + 2 T3 Sin@phi@tDD, Fphi, - T2 + T3<<
In[231]:=
H* Equations of motion *L
In[232]:=
eqmot = SetEqual@rhs, lhsD;
5
6
example.nb
In[233]:=
In[234]:=
Out[234]=
In[235]:=
Out[235]=
In[236]:=
Out[236]=
H* For thinking about control systems, break the forces into three components,
corresponding to being able to control T1, T2, and T3. *L
F1 = Coefficient@F, T1D
81, 0, 0<
F2 = Coefficient@F, T2D
80, 0, - 1<
F3 = Coefficient@F, T3D
82 Sin@phi@tDD, 0, 1<
In[237]:=
H* Vector fields corresponding to control forces *L
In[238]:=
Y1 = [email protected];
In[239]:=
Y2 = [email protected];
In[240]:=
Y3 = [email protected];
In[241]:=
H* Sample Lie bracket: @Y1,Y3D *L
In[242]:=
In[243]:=
In[244]:=
Y1bracketY3 = Table@Sum@D@Y3@@jDD, q@@kDDD Y1@@kDD, 8k, 1, 3<D Sum@D@Y1@@jDD, q@@kDDD Y3@@kDD, 8k, 1, 3<D, 8j, 1, 3<D;
H* Sample covariant derivative: \nabla_Y1 Y2 *L
Y1covariantY2 = Table@Sum@D@Y2@@jDD, q@@kDDD Y1@@kDD, 8k, 1, 3<D +
Sum@connection@@jDD@@kDD@@lDD Y1@@kDD Y2@@lDD, 8k, 1, 3<, 8l, 1, 3<D, 8j, 1, 3<D;