# Difficulty in Animation of Inverted Pendulum from Interpolating Function

I was trying to model an inverted pendulum system and control it. I successfully got the plot but I'm not able to use the data to animate a graphically created inverted pendulum. Run the code serially, you'll get the output.

These are the parameters I used:

pars = {L -> 1, g ->  9.81, m ->  0.2, M ->  0.3};


This is the system of equations:

eqns = {(M + m) x''[t] + m L Sin[θ[t]] θ'[t]^2 -
m L Cos[θ[t]] θ''[t] == F[t],
L θ''[t] - x''[t] Cos[θ[t]] - g Sin[θ[t]] ==
0};


This is the gain I calculated by setting the poles:

k = StateFeedbackGains[StateSpaceModel[pend /. pars], {-2, -3 + I, -3 - I, -4}]


This is the final transfer function:

csys = SystemsModelStateFeedbackConnect[pend, k]


Now, finally the plot was plotted showing the controlling effect:

T = 5;
res = OutputResponse[{csys /. pars, {(Pi/4), 0, 0, 0}}, 0, {t,0,T}];
Plot[res, {t, 0, T}, PlotRange -> All, PlotLegends -> {θ[t], x[t]}]


I am trying to simulate this plot:

L = 1;
Manipulate[
Graphics[
{
{Line[{{0, 0}, {L Sin[s], L Cos[s]}}]},
{Line[{{-L/4, 0}, {L/4, 0}}]},
{Disk[{L Sin[s], L Cos[s]}, 0.05]},
{Dashed, Gray, Line[{{0, 0}, {0, L}}]},
{Gray, Rectangle[{-L/4 - 1/4, -1/15}, {L/4 + 1/4, 1/15}]}
}, ImageSize -> Medium,
PlotRange -> {{-L - 1, L + 1}, {-L - 1, L + 1}}], {s, 0, 2 Pi}]


I know how to simulate this plot if it were calculated from a given differential equation but this plot is numerically calculated. How to use its data from the res function to simulate the pendulum? I can extract one single plot from the 2 plots generated by res[].

The Inverted pendulum is a nonlinear system I assume "Pend" is your SystemStateModel or Transferfunction, which is linearized and used for calculating your gains and extra to give you an Interpolating function. Pend, however is missing from your code, so I've put an example and I think the best method for animating your system.

...I assume this is a pendulum cart by looking at your equation.

Here is an example way to plot using the results from an NDSolve[] Interpolated function with a controlling force. (done via LQR) this code is easily modified in DrawingSinglePendulum[] under Graphics, to fit your coordinate system.

Animation code:

AnimatePendulum[rules_] :=
Animate[Evaluate[
DrawSinglePendulum[x[t] /. rules, {\[Theta][t] /. rules, 1, 1},
t]], {t, 0, Max[First[x /. rules]]}, DefaultDuration -> 15,
AnimationRunning -> False]

DrawSinglePendulum[cart_, {angle1_, length1_, mass1_}, t_] :=

Module[{width1, density = 30},
width1 = mass1/length1/density;
Graphics[
{
{Blue, Rectangle[{cart - 1/4, -1/15}, {cart + 1/4, 1/15}]},
{Orange,
Translate[
Rotate[Rectangle[{0, width1}, {length1, -width1}],
angle1, {0, 0}], {cart, 0}]}
},
PlotRange -> 2, ImageSize -> 280,
Frame -> False, Axes -> False, AxesStyle -> Dashed,
PlotLabel -> "t" == NumberForm[t, {4, 2}]
]
]

SimulatePendulum[force_] :=
AnimatePendulum[
First[NDSolve[
Join[eqns /.
f[t] -> force, {x == x' == \[Theta]' ==
0, \[Theta] == \[Pi]/2 - 0.1}], {x, \[Theta]}, {t, 0, 30}]]]


This code I wish i could credit as my own, but is infact an example code taken from none other than Wolfram themselves. I find it a great example, and have used this method to animate many other controlling projects I've had/have.

A working Example Inverted Pendulum cart is below, this is the same as in the blog post, though I've modified it some...you can use "sol" in simulate pendulum to show the lack of control..Or even just "0"

bumps[t_] = 10 Exp[-10 (t - 8)^2] - 10 Exp[-10 (t - 16)^2];
eqns = {2 f[t] + Cos[\[Theta][t]] Derivative[\[Theta]][t]^2 +
Sin[\[Theta][t]] (\[Theta]^\[Prime]\[Prime])[t] ==
4 (x^\[Prime]\[Prime])[t],
2 Cos[\[Theta][t]] -
2 Sin[\[Theta][t]] (x^\[Prime]\[Prime])[
t] + (\[Theta]^\[Prime]\[Prime])[t] == 0};
sol = NDSolve[
Join[eqns /.
f[t] -> 0, {x == x' == \[Theta]' ==
0, \[Theta] == \[Pi]/2 - 0.1}], {x, \[Theta]}, {t, 0, 30}]
pen = { Cos[\[Theta][t]], Sin[\[Theta][t]]} /. sol
ParametricPlot[pen, {t, 0, 4}, PlotRange -> All, ImageSize -> Medium,PlotLegends -> "Expressions", PlotStyle -> Dashed, AxesLabel -> {x, y}] just a plot to make sure we have something realistic...

model = StateSpaceModel[
eqns, {{x[t], 0}, {x'[t], 0}, {\[Theta][t], \[Pi]/2}, {\[Theta]'[t],
0}}, f[t], {}, t]
gains = LQRegulatorGains[
N[model], {DiagonalMatrix[{1, 10, 10, 100}], {{1}}}]

controlforce =
First[-gains].{x[t], x'[t], \[Theta][t] - \[Pi]/2, \[Theta]'[t]}

SimulatePendulum[controlforce + bumps[t]] And an example from a flywheel pendulum, I've done before to show how easily modifiable the code is.

DrawSinglePendulum[\[Theta]_, \[Phi]_, t_] :=
Graphics[{{Black,
Rotate[Rectangle[{0, 0}, {1.5, 1.5}], \[Theta] - (3 \[Pi])/4, {0,
0}]}, Arrow[{{2, 2}, {2, 1.5}}], {Thickness[0.01], Red,
Rotate[Circle[{0, 1/2 1.5 Sqrt Cos[\[Pi]]}, {0.45,
0.45}], \[Theta], {0, 0}]}, {Thickness[0.01],
Rotate[Rotate[{Purple,
Line[{{0, -1.5 Cos[\[Pi]/4]}, {0.45,
1/2 (-1.5) Sqrt}}]}, \[Theta], {0, 0}], \[Phi] - \[Pi]/6,
RotationTransform[\[Theta]][{0,
1/2 (-1.5) Sqrt}]]}, {Thickness[0.01],
Rotate[Rotate[{Red,
Line[{{0, -1.5 Cos[\[Pi]/4]}, {0, -1.5 Cos[\[Pi]/4] +
0.45}}]}, \[Theta], {0, 0}], \[Phi],
RotationTransform[\[Theta]][{0,
1/2 (-1.5) Sqrt}]]}, {Thickness[0.01],
Rotate[Rotate[{Red,
Line[{{0, -1.5 Cos[\[Pi]/4]}, {0, -1.5 Cos[\[Pi]/4] +
0.45}}]}, \[Theta], {0, 0}], \[Phi] + (2 \[Pi])/3,
RotationTransform[\[Theta]][{0, 1/2 (-1.5) Sqrt}]]}},
PlotRange -> 3, ImageSize -> 400, Frame -> True,
FrameLabel -> {x, z}, Axes -> True,
AxesStyle -> {{Dashed, Gray}, {Dashed, Gray}},
PlotLabel -> "t" == NumberForm[t, {4, 2}]] Hope this helps you along!