Wolfram Computation Meets Knowledge

Stabilized n-Link Pendulum

In the previous post in this series, we looked at how to model a stabilized inverted pendulum using the control systems design features in Mathematica 8. We were quickly able to simulate a linearly controlled cart-and-pendulum system, and show that it is stable against some fairly large perturbations.

But what about a double (or triple or quadruple… ) pendulum? A general n-link pendulum is depicted below. In this post we’ll see how to derive the equations of motions for this system, find out whether we can stabilize it with a linear control scheme, and produce some animations of the results.

General n-link pendulum

For the single pendulum case from the previous post we derived the equations of motion manually, but for the n-link case it is much easier to derive them programmatically. I’ll show one way to do this step by step, using a Lagrangian mechanics formulation. If you prefer, you could easily skip ahead from here to the control theory portion of the post.

The n + 1 generalized coordinates describing the n-link pendulum depicted above are the cart position x(t) and the angles of each arm θi(t):

n+1 generalized coordinates

The n + 1 generalized velocities are the time derivatives of the coordinates:

Time derivatives of the coordinates

Together these comprise the dynamical variables of the n-link pendulum. For the double (“2-link”) pendulum, for example, the dynamical variables are:

Dynamical variables

Output of dynamical variables

The n + 1 generalized external forces are just the control force f(t) acting on the cart and zero forces acting on each of the other n generalized coordinates:

Zero forces

For example, the generalized external forces acting on the triple pendulum are:

External forces acting on the triple pendulum

External forces acting on the triple pendulum output

Now we need to compute the Lagrangian of the n-link pendulum. It helps to define a couple of other properties of the system first, starting with the location of the hinge of each of the n arms. The hinge of the first arm is at the horizontal position of the cart, with a height of zero:

Hinge of first arm

The location of the hinge of each other arm depends on the location of the previous hinge in the following simple way:

Hinge of other arm

For example, the hinge of the second arm is at the end of the first arm:

Hinge of second arm

Hinge of the second arm is at the end of the first arm

We assume the arms have uniform linear density, so the center of mass of the ith arm is halfway along it:

Center of mass of the ith arm is halfway along the other arm

The kinetic energy of the ith arm is 1/2 mi v.v, where v is the velocity of its center of mass:

Kinetic energy of the arm

In our dimensionless units in which the gravitational acceleration g = 1, the potential energy of the ith arm is its mass multiplied by the height of its center of mass:

Potential energy of the arm

(The 〚2〛 gives the second component of armcenter[i], which is the height.)

The Lagrangian of the cart is just its kinetic energy, cx′(t)2⁄2, so the overall Lagrangian of the n-link pendulum is:

Overall Lagrangian of the n-link pendulum

Now we are ready to apply the Euler–Lagrange equations and get the equations of motion:

Euler-Lagrange equations

(The MapThread function gives the Euler–Lagrange equation, Euler-Lagrange equation, for each corresponding generalized coordinate q, generalized velocity v, and generalized force h.)

OK! After a bit of work, we have the equations of motion for any n-link pendulum. Let’s take a look at a few of them.

For n = 0 (a cart without any pendulum), we just get Newton’s second law relating the cart’s mass c and acceleration x″(t) to the control force f(t):

Newton's second law

Relating mass and acceleration to control force

So far, so good. For n = 1 we get the same equations of motion as we used in the first post in this series:

Equations of motion

Equations of motion output

For n = 2, a cart-and-double-pendulum system, the explicit equations of motion are fairly complicated:

Equations of motion for n=2

Equations of motion for n=2 output

The equations become much longer for larger n, but we will happily use them without ever looking at them.

(If you do want to see the equations of motion for, say, a quadruple pendulum printed out in full, just download the Computable Document Format (CDF) file for this post and evaluate equations[4]. The output is about five pages long on-screen, which is exactly why you don’t want to derive these things by hand.)

We used symbolic math and programming to set up the equations of motion. Now we can immediately use Mathematica‘s numerics and control systems features to simulate and stabilize them. This is what’s great about having all these areas integrated into one system!

We’ll investigate the concrete case c = mi = di = 1:

Parameters

(The mi_ means “mi for all values of i”.)

It’s handy to define a function initial[n] that generates some slightly non-upright initial conditions at t = 0 for an n-link pendulum. For example, here are slightly non-upright initial conditions for a single pendulum:

Initial conditions for a single pendulum

Initial conditions for a single pendulum output

Note that the pendulum is exactly upright when each θi = π ⁄ 2 ≈ 1.5708. (Download the CDF file to see the definition of initial[n], or to substitute your own.)

We’ll start by simulating a double pendulum with no control force, f(t) = 0, corresponding to making no attempt to balance the pendulum:

Double pendulum with no control force

Double pendulum solution

Here’s an animation of the solution:

Animate first pendulum

(As in the previous post, you can see the definition of the function AnimateNPendulum by downloading the CDF file.)

Once again, it’s convenient to combine the above steps into a single function:

Triple pendulum no control

Using this function, here’s a triple pendulum falling over, with no attempt to control it:

Animate second pendulum

It gets messy fast, but we’ll be able to steady it!

First, we need a state space model of the n-link pendulum, linearized around the equilibrium point. The function StateSpaceModel automatically generates this for us from our nonlinear equations of motion:

StateSpaceModel function

The function equilibrium[n] just gives the unstable (upright) equilibrium point for an n-link pendulum: θi = π ⁄ 2; everything else = 0. For example, for a double pendulum:

Equilibrium function for a double pendulum

Definition of equilibrium[2]

(Download the CDF file to see the definition of equilibrium[n].)

Here is the linearized state space model for our double pendulum:

model[2]

Linearized state space model for double pendulum

As we discussed in the previous post, each column in this augmented matrix describes the immediate effect of one of the dynamical variables (or the control force) on each of the other dynamical variables.

We can now compute a linear control force for this system. For a double pendulum, for example, the linear control force will have the form

Linear control force for double pendulum

LQRegulatorGains gives values for the feedback gains ki:

LQRegulatorGains function

Values for feedback gains

As discussed in the previous post, the second argument to LQRegulatorGains specifies a quadratic cost function whose integral over time is minimized (in the linearized model). The exact cost function specified above is

Exact cost function

The coefficients in the quadratic cost function may represent actual costs of deviations of the system from the desired equilibrium state, or they may be chosen according to other criteria, such as robustness against certain types of perturbations. The cost coefficients I’ve used here are a guess, based on which dynamical variables I suspect are the most important for keeping the pendulum from falling over.

Here is a generalization of my guessed cost coefficients to an n-link pendulum:

Generalization of guessed cost coefficients

The corresponding feedback gains are:

Corresponding feedback gains

And the control force is:

Control force for n-link pendulum

Our control force for a double pendulum is:

Control force for double pendulum

Control force for double pendulum output

How well does it work?

Animate third pendulum

Very graceful! Here’s the stabilized triple pendulum (compare it to the freely falling triple pendulum above):

Animate fourth pendulum

Some pretty nifty maneuvering is needed to catch the quadruple pendulum before it falls over:

Animate fifth pendulum

Let’s see what sort of bumping around the double pendulum can handle. We’ll apply generalized forces h1(t) and h2(t) to the coordinates θ1(t) and θ2(t) respectively. (We are effectively applying torques to the first and second hinges of the pendulum.)

Apply torques to first and second hinges of pendulum

(Download the CDF file to see the definitions of h1(t) and h2(t), or to substitute your own.)

Here’s the bumped double pendulum:

It’s also interesting to directly plot the generalized coordinates x(t), θ1(t), and θ2(t), and compare with the forces h1(t) and h2(t) and the control force f(t):

Plot generalized coordinates

With sufficiently large perturbations, linear control systems such as the ones we have been exploring will fail to recover (and will spiral out of control). Moreover, for n-link pendulums, larger values of n require smaller angular perturbations to knock them over, and in real life there are always perturbations (sound, heat, wind… ). In practice it’s hard to build physical demonstrations of n-link pendulums with more than about three or four links.

Nevertheless, it’s very interesting to explore these idealized systems. There are lots of things you can try with the downloadable CDF file for this post. For example, with different choices for the cost function, you can make linear control systems for inverted n-link pendulums that can recover from different (including larger) initial perturbations. Or, with different lengths and masses of the pendulum arms (and no control force), you can explore the dynamics of n-link pendulums, which are very interesting by themselves. Try it out!

In the next post in this series, we’ll look at some more generalizations and variations on this problem, including inverted pendulums in 3D.

Download the CDF file

Comments

Join the discussion

!Please enter your comment (at least 5 characters).

!Please enter your name.

!Please enter a valid email address.

11 comments

  1. It seems that the angular kinetic energy is missing :
    sum of 1/2 J thetadot^2 for each link …
    This could explain the strange non realistic behaviour of the free pendulum animations.
    Friction could also be added to be more realistic. It is also good for stabilization and will give better looking free system animations.

    Nicolas

    Reply
    • I just came to ask on this issue of missing rotational energy.
      In principle, the center of gravity of one of the links can stay at a constant height at zero speed.
      Still, the link will have a kinetic energy larger than zero as it rotate.
      This said, I enjoyed reading this impressive blog.

      Reply
  2. Who’d have thought you could use the EL equations and M to teach you breakdance?

    Great post, Andrew, again!

    Reply
  3. Hi Nicolas, We could have alternatively derived the equations by adding the overall translational kinetic energy to the rotational kinetic energy. We would end up with the same equations. Including friction is something we will try in the next post in this series!

    Reply
  4. Hi Andrew,
    yes the equations are right, but the hypothesis that all the arm mass is located in its middle, and has no rotational inertia is just unrealistic. You may change the drawing to make it fit your hypothesis. This means to not display arms by solid beams but rather by thin lines with a (disk) mass in the middle. In this case the animation will be realistic, nevertheless the real experiment would appear less realistic unless you use carbon fiber rods and lead masses located in their middle.

    If you are not convinced, take a ruler by its middle between two fingers, move it to any direction and you can fill its translational inertia. If the ruler would have all its mass in the middle, you would fill exactly the same, right ? Now use your finger to give a torque to the ruler, you feel its rotational inertia, this important dynamic effect is missing when you assume that all the mass is located in the middle point.

    If you would like to add rational inertial effect, you may create a lumped approximation by a repartition of fraction of masses along each arm (at least two), and use the same equations, or formally include rotational inertia of the arms in the Lagrangian.

    Reply
  5. Hi Nicolas, you’re right! That’s more realistic, especially considering my diagrams. To make this change we have to add the following term to the definition of ke[i_]:

    + (Subscript[m, i] Subscript[d, i]^2)/24 Subscript[\[Theta], i]'[t]^2

    I will update the downloadable notebook for this post to include this option. Thanks!

    Reply
  6. This was a fascinating topics with amazing graphics.

    Idea for a future column: numerically solve the equations of the “Tippe Top”, along with a graphic of a spinning top to illustrate the solution.

    Reply
  7. Fantastic post! I really liked that you were able to showcase the symbolic power of Mathematica along with the new features of v8. Great job.

    Reply
  8. This blog post was phenomenal! Thank you so much. It made me see how powerful Mathematica can be.

    Reply
  9. This post very interest for me !
    Folowing your work , I tested my folowing code , in Mathematica 10.02, using NDSolve , it works finely :

    Clear[initial]
    initial[n_] :=
    Flatten[{x[0] == x'[0] == 0,
    Table[{Subscript[\[Theta], i][0] == N[ \[Pi]/2 – 1/35 (Sqrt[2])^i],
    Subscript[\[Theta], i]'[0] == RandomReal[{-0.1, 0.1}]}, {i, n}]}]
    k = 10;
    linex = {{-10, 5}, {10, 5}};
    liney = {{0, 5}, {0, -15}};
    Join[equations[3], initial[3]] /. parameters /. f[t] -> Sin[2 t]

    sol = NDSolve[
    Join[equations[k], initial[k]] /. parameters /. f[t] -> Sin[2 t],
    coordinates[k], {t, 0, 25},
    Method -> {“EquationSimplification” -> “Residual”}];
    x[t_] := Evaluate[x[t] /. sol[[1, 1]]]
    Subscript[\[Theta], 1][t_] :=
    Evaluate[Subscript[\[Theta], 1][t] /. sol[[1, 2]]]
    Subscript[\[Theta], 2][t_] :=
    Evaluate[Subscript[\[Theta], 2][t] /. sol[[1, 3]]]
    Subscript[\[Theta], 3][t_] :=
    Evaluate[Subscript[\[Theta], 3][t] /. sol[[1, 4]]]
    Subscript[\[Theta], 4][t_] :=
    Evaluate[Subscript[\[Theta], 4][t] /. sol[[1, 5]]]
    Subscript[\[Theta], 5][t_] :=
    Evaluate[Subscript[\[Theta], 5][t] /. sol[[1, 6]]]
    Subscript[\[Theta], 6][t_] :=
    Evaluate[Subscript[\[Theta], 6][t] /. sol[[1, 7]]]
    Subscript[\[Theta], 7][t_] :=
    Evaluate[Subscript[\[Theta], 7][t] /. sol[[1, 8]]]
    Subscript[\[Theta], 8][t_] :=
    Evaluate[Subscript[\[Theta], 8][t] /. sol[[1, 9]]]
    Subscript[\[Theta], 9][t_] :=
    Evaluate[Subscript[\[Theta], 9][t] /. sol[[1, 10]]]
    Subscript[\[Theta], 10][t_] :=
    Evaluate[Subscript[\[Theta], 10][t] /. sol[[1, 11]]]
    fr = Table[p = {x[t], 5};
    plist = Join[{p},
    Table[p +=
    Evaluate[{Sin[
    Subscript[\[Theta], i][t]], -Cos[
    Subscript[\[Theta], i][t]]}], {i, 1, 10}]];
    Show[Graphics[{Blue, Thick, Line[linex], Blue, Thick, Line[liney],
    Red, Line[plist], Black, Map[Disk[#, 0.15] &, plist]}],
    PlotRange -> 10*1 {{-1, 1}, {-1, 1}},
    AspectRatio -> Automatic], {t, 0.0, 25.0, 0.03}];
    ListAnimate[fr]

    Now ,I use NDSolveValue , it not work , Why ? Where my mistake :

    Clear[initial]
    initial[n_] :=
    Flatten[{x[0] == x'[0] == 0,
    Table[{Subscript[\[Theta], i][0] == N[ \[Pi]/2 – 1/35 (Sqrt[2])^i],
    Subscript[\[Theta], i]'[0] == RandomReal[{-0.1, 0.1}]}, {i, n}]}]
    k = 10;
    linex = {{-10, 5}, {10, 5}};
    liney = {{0, 5}, {0, -15}};
    Join[equations[3], initial[3]] /. parameters /. f[t] -> Sin[2 t]

    {x, Subscript[\[Theta], 1], Subscript[\[Theta], 2], \
    Subscript[\[Theta], 3], Subscript[\[Theta], 4], Subscript[\[Theta], \
    5], Subscript[\[Theta], 6], Subscript[\[Theta], 7], \
    Subscript[\[Theta], 8], Subscript[\[Theta], 9], Subscript[\[Theta], \
    {x, Subscript[\[Theta], 1], Subscript[\[Theta], 2], \
    Subscript[\[Theta], 3], Subscript[\[Theta], 4], Subscript[\[Theta], \
    5], Subscript[\[Theta], 6], Subscript[\[Theta], 7], \
    Subscript[\[Theta], 8], Subscript[\[Theta], 9], Subscript[\[Theta], \
    {x, Subscript[\[Theta], 1], Subscript[\[Theta], 2], \
    Subscript[\[Theta], 3], Subscript[\[Theta], 4], Subscript[\[Theta], \
    5], Subscript[\[Theta], 6], Subscript[\[Theta], 7], \
    Subscript[\[Theta], 8], Subscript[\[Theta], 9], Subscript[\[Theta], \
    {x,Subscript[\[Theta], 1],Subscript[\[Theta], 2],Subscript[\[Theta], 3],Subscript[\[Theta], 4],Subscript[\[Theta], 5],Subscript[\[Theta], 6],Subscript[\[Theta], 7],Subscript[\[Theta], 8],Subscript[\[Theta], 9],Subscript[\[Theta], 10]} =
    NDSolveValue[
    Join[equations[k], initial[k]] /. parameters /. f[t] -> Sin[2 t],
    coordinates[k], {t, 0, 25},
    Method -> {“EquationSimplification” -> “Residual”}];
    fr = Table[p = {x[t], 5};
    plist = Join[{p},
    Table[p +=
    Evaluate[{Sin[
    Subscript[\[Theta], i][t]], -Cos[
    Subscript[\[Theta], i][t]]}], {i, 1, 10}]];
    Show[Graphics[{Blue, Thick, Line[linex], Blue, Thick, Line[liney],
    Red, Line[plist], Black, Map[Disk[#, 0.15] &, plist]}],
    PlotRange -> 10*1 {{-1, 1}, {-1, 1}},
    AspectRatio -> Automatic], {t, 0.0, 25.0, 0.03}];
    ListAnimate[fr]

    Please , help me . Thank you very much

    Reply
  10. The NDsolve does not seem to work for n=3 and shows the following error: NDSolve::ntdv: Cannot solve to find an explicit formula for the derivatives. Consider using the option Method->{“EquationSimplification”->”Residual”}. >>
    could you help with solving this problem?
    Thank You.

    Reply