- #1
AtLastForgot
- 2
- 0
I'm trying to use Mathematica to solve a system of equations based on coordinate transformations and Lagrangian mechanics. The code follows here. However, when the system of equations is run, Mathematica never reaches an answer nor does it give an error, it simply runs infinitely. This is for a class final project that's worth a large portion of my grade.
(*Definition of various functions which will help us simplify our \
code later. None of this is physically relevant; it is simply a \
convenience.*)
g[x_, y_, \[Theta]_] := {{Cos[\[Theta]], -Sin[\[Theta]],
x}, {Sin[\[Theta]], Cos[\[Theta]], y}, {0, 0, 1}}
unhat[a_] := {a[[1, 3]], a[[2, 3]], a[[2, 1]]}
hat[a_] := {{0, -a[[3]], a[[1]]}, {a[[3]], 0, a[[2]]}, {0, 0, 0}}
Hom[point_] := Flatten[{point, 1}]
Unhom[point_] := {point[[1]], point[[2]]}
Clear[m1, m2, m3, J1, J2, J3, L1, L2, L3, grav]
(*The first transformation from an origin of 0,0 to the primary axes \
of the first bar.*)
gUpperArm[x_, y_, \[Theta]1_] := g[x, y, 0].g[0, 0, \[Theta]1];
(*Velocity of this point is determined both in terms of translational \
velocities as well as rotational. Unhat allows us to calculate this \
quantity, normally [g^-1][g'], via simple notation.*)
VUpperArm =
unhat[Inverse[gUpperArm[x[t], y[t], \[Theta]1[t]]].D[
gUpperArm[x[t], y[t], \[Theta]1[t]], t]];
(*Transformation from origin to the primary axes of the second bar \
using intermediate steps of the first transformation. This links \
their two motions and allows us to calculate VForearm, a quantity \
which would otherwise be extremely difficult to describe.*)
gForearm[x_, y_, \[Theta]1_, \[Theta]2_] :=
gUpperArm[x, y, \[Theta]1].g[L1/2, 0, 0].g[0, 0, \[Theta]2].g[L2/2,
0, 0];
VForearm =
unhat[Inverse[gForearm[x[t], y[t], \[Theta]1[t], \[Theta]2[t]]] .
D[gForearm[x[t], y[t], \[Theta]1[t], \[Theta]2[t]], t]];
gWeapon[x_, y_, \[Theta]1_, \[Theta]2_, \[Theta]3_] :=
gForearm[x, y, \[Theta]1, \[Theta]2].g[L2/2, 0, 0].g[0,
0, \[Theta]3].g[L3/4, 0, 0];
VWeapon =
unhat[Inverse[
gWeapon[x[t], y[t], \[Theta]1[t], \[Theta]2[t], \[Theta]3[t]]] .
D[gWeapon[x[t], y[t], \[Theta]1[t], \[Theta]2[t], \[Theta]3[t]],
t]];
(*Definitions of inertia for the various components of the system. \
Notice that the form is x,y,\[Theta].*)
IUpperArm = DiagonalMatrix[{m1, m1, J1}];
IForearm = DiagonalMatrix[{m2, m2, J2}];
IWeapon = DiagonalMatrix[{m3, m3, J3}];
(*Calculations of Kinetic Energy using previously defined \
transformations to make this otherwise nearly impossible task much \
simpler.*)
KE = ((1/2)*VUpperArm.IUpperArm.VUpperArm) + ((1/2)*
VForearm.IForearm.VForearm) + ((1/2)*VWeapon.IWeapon.VWeapon);
V = m1*grav*(gUpperArm[x[t], y[t], \[Theta]1[t]].{0, 0, 1})[[2]] +
m2*grav*(gForearm[x[t], y[t], \[Theta]1[t], \[Theta]2[t]].{0, 0,
1})[[2]] +
m3*grav*(gWeapon[x[t],
y[t], \[Theta]1[t], \[Theta]2[t], \[Theta]3[t]].{0, 0, 1})[[
2]];
Lag = KE - V;
EQ1 = D[D[Lag, x'[t]], t] - D[Lag, x[t]];
EQ2 = D[D[Lag, y'[t]], t] - D[Lag, x[t]];
EQ3 = D[D[Lag, \[Theta]1'[t]], t] - D[Lag, \[Theta]1[t]];
EQ4 = D[D[Lag, \[Theta]2'[t]], t] - D[Lag, \[Theta]2[t]];
EQ5 = D[D[Lag, \[Theta]3'[t]], t] - D[Lag, \[Theta]3[t]];
EQ = Solve[{EQ1 == 0, EQ2 == 0, EQ2 == 0, EQ4 == 0, EQ5 == 0}, {x[t],
y[t], \[Theta]1[t], \[Theta]2[t], \[Theta]3[t]}];
(*Definition of various functions which will help us simplify our \
code later. None of this is physically relevant; it is simply a \
convenience.*)
g[x_, y_, \[Theta]_] := {{Cos[\[Theta]], -Sin[\[Theta]],
x}, {Sin[\[Theta]], Cos[\[Theta]], y}, {0, 0, 1}}
unhat[a_] := {a[[1, 3]], a[[2, 3]], a[[2, 1]]}
hat[a_] := {{0, -a[[3]], a[[1]]}, {a[[3]], 0, a[[2]]}, {0, 0, 0}}
Hom[point_] := Flatten[{point, 1}]
Unhom[point_] := {point[[1]], point[[2]]}
Clear[m1, m2, m3, J1, J2, J3, L1, L2, L3, grav]
(*The first transformation from an origin of 0,0 to the primary axes \
of the first bar.*)
gUpperArm[x_, y_, \[Theta]1_] := g[x, y, 0].g[0, 0, \[Theta]1];
(*Velocity of this point is determined both in terms of translational \
velocities as well as rotational. Unhat allows us to calculate this \
quantity, normally [g^-1][g'], via simple notation.*)
VUpperArm =
unhat[Inverse[gUpperArm[x[t], y[t], \[Theta]1[t]]].D[
gUpperArm[x[t], y[t], \[Theta]1[t]], t]];
(*Transformation from origin to the primary axes of the second bar \
using intermediate steps of the first transformation. This links \
their two motions and allows us to calculate VForearm, a quantity \
which would otherwise be extremely difficult to describe.*)
gForearm[x_, y_, \[Theta]1_, \[Theta]2_] :=
gUpperArm[x, y, \[Theta]1].g[L1/2, 0, 0].g[0, 0, \[Theta]2].g[L2/2,
0, 0];
VForearm =
unhat[Inverse[gForearm[x[t], y[t], \[Theta]1[t], \[Theta]2[t]]] .
D[gForearm[x[t], y[t], \[Theta]1[t], \[Theta]2[t]], t]];
gWeapon[x_, y_, \[Theta]1_, \[Theta]2_, \[Theta]3_] :=
gForearm[x, y, \[Theta]1, \[Theta]2].g[L2/2, 0, 0].g[0,
0, \[Theta]3].g[L3/4, 0, 0];
VWeapon =
unhat[Inverse[
gWeapon[x[t], y[t], \[Theta]1[t], \[Theta]2[t], \[Theta]3[t]]] .
D[gWeapon[x[t], y[t], \[Theta]1[t], \[Theta]2[t], \[Theta]3[t]],
t]];
(*Definitions of inertia for the various components of the system. \
Notice that the form is x,y,\[Theta].*)
IUpperArm = DiagonalMatrix[{m1, m1, J1}];
IForearm = DiagonalMatrix[{m2, m2, J2}];
IWeapon = DiagonalMatrix[{m3, m3, J3}];
(*Calculations of Kinetic Energy using previously defined \
transformations to make this otherwise nearly impossible task much \
simpler.*)
KE = ((1/2)*VUpperArm.IUpperArm.VUpperArm) + ((1/2)*
VForearm.IForearm.VForearm) + ((1/2)*VWeapon.IWeapon.VWeapon);
V = m1*grav*(gUpperArm[x[t], y[t], \[Theta]1[t]].{0, 0, 1})[[2]] +
m2*grav*(gForearm[x[t], y[t], \[Theta]1[t], \[Theta]2[t]].{0, 0,
1})[[2]] +
m3*grav*(gWeapon[x[t],
y[t], \[Theta]1[t], \[Theta]2[t], \[Theta]3[t]].{0, 0, 1})[[
2]];
Lag = KE - V;
EQ1 = D[D[Lag, x'[t]], t] - D[Lag, x[t]];
EQ2 = D[D[Lag, y'[t]], t] - D[Lag, x[t]];
EQ3 = D[D[Lag, \[Theta]1'[t]], t] - D[Lag, \[Theta]1[t]];
EQ4 = D[D[Lag, \[Theta]2'[t]], t] - D[Lag, \[Theta]2[t]];
EQ5 = D[D[Lag, \[Theta]3'[t]], t] - D[Lag, \[Theta]3[t]];
EQ = Solve[{EQ1 == 0, EQ2 == 0, EQ2 == 0, EQ4 == 0, EQ5 == 0}, {x[t],
y[t], \[Theta]1[t], \[Theta]2[t], \[Theta]3[t]}];