Teaching a Double Pendulum to Stand Up

The Deceptively Complicated Machine

Balancing a broom on your palm is a task most people are capable of doing. You watch the top, and when it tips one way you instinctively move your hand that way to get back under it.

If you then cut that broom in half and join the two pieces with a spinning joint the task undoubtedly becomes much harder. The top piece does whatever it likes, and the only way you can influence it is by maneuvering the bottom piece around underneath it. That is the premise of this machine (or plant: In control theory, a plant is the physical system, machine, or process whose behavior you want to control. ).

The state: Everything you need to know about the machine right now in order to predict what it does next. Anything not in the state cannot be reacted to. of the machine is captured by six numbers, the cart's position and velocity, and the angle and spin rate of each link. Angles are measured from straight up, so θ = 0 is balanced and θ = π is hanging straight down. The single input is a horizontal force on the cart.

To keep track of the system we can think back to the absolute basics of physics, F = ma. That simple equation works for a simple single body system and if we slightly adapt it, it will work for the double pendulum too. The main difference for the pendulum is that each symbol in the equation gets promoted from a single number to a small table, because now three things accelerate at once and push on each other. We also have to introduce new force terms to account for the fact that the links are swinging and gravity is pulling on them. The resulting equation looks like this:

We'll need to expand these terms for the calculations that the computer will do. That can be seen below.

That became an alphabet soup real fast but don't let that discourage you! The underlying concepts are still quite simple here. $u$ is the one lonely motor, which is only in the first row because it can only push the cart along one dimension. $C(q,\dot{q})\dot{q}$ is the whipping, the centrifugal and Coriolis: Apparent forces that only show up because parts of the machine are already moving. They are why a spinning object tugs sideways, and why a fast linkage behaves nothing like a slow one. forces. $G(q)$ is gravity, pulling the links downwards. And finally, $M(q)$ is just the inertia represented as a table rather than a single mass, because shoving the cart also swings the links.

This equation will be the basis for the simulation, it's what is used to calculate how the system is working on every tick. The engineering task then becomes conceptually dead simple, figure out what to make $u$ so that the pendulum does what we want it to do.

The Balancing Act

Let's start with the good news: when the pendulum is nearly upright we can simplify it to a linear control problem. When the tilt angles stay small near the equilibrium: A pose where the machine will stay put if undisturbed, because all the forces cancel. Hanging straight down is a stable one; balanced upright is an unstable one, where the smallest nudge grows. , the math simplifies and the curves behave almost like straight lines, since sin θ ≈ θ. Basically, when the pendulum is standing up, it acts more like a rigid stick. Linearizing: Replacing the true curved physics with the straight-line approximation that is accurate near one particular pose. It is exact at that pose and gets steadily more wrong as you move away from it. like this leaves a plain linear system, which lets us solve for the best possible motor force to apply using the Linear Quadratic Regulation (LQR) control algorithm.

LQR works with the concepts of penalties for things you don't want to happen. Being off-center is a penalty. Being tilted is a bigger penalty. Using force can be a penalty too, because a real motor has limits and you don't want your controller to be super violent. If we integrate (accumulate) those penalties continuously over all future time, we get a single total cost score, J, to minimize:

$Q$ and $R$ are what we get to play with here to control the system. If we raise $R$ the controller becomes lazy and gentle, raise $Q$ and it gets twitchy and aggressive. I bring even more good news now: because this is a linear system this problem has an exact solution, and we can find it by solving the Riccati equation: A matrix equation that shows up throughout optimal control and estimation. Solving it for its unknown matrix gives the best possible trade-off between two competing costs, without any search or iteration. . At first, the Riccati equation will feel like a blender of five different matrices (A, B, P, Q, R) with transposes and inverses thrown in but I will explain it so it makes more sense.

This is where the sausage gets made. We feed in the machine's linearized physics with $A$ (how the plant drifts on its own) and $B$ (how the motor pushes the plant) along with the chosen penalties ($Q$ and $R$). Plugging in the knowns, we solve for matrix $P$ which captures the total future cost of being in any given state.

One way to read the $P$ on both sides of that middle term is as cause and effect. $P$ is the bill for future cost. The first $P$ sets how hard the motor should push right now. That push moves the machine and changes how much future penalty we'll have, and the second $P$ prices that change. Because how hard we push depends on $P$, and the consequence of that push also scales with $P$, the two multiply together into $-P(B{R}^{-1}{B}^{\top })P$.

It isn't a strict derivation, but it's the intuition for if it spends a little motor effort now it won't have to pay for a big correction later. After we solve for $P$, the gains needed for the controller are computed with a simple matrix multiplication:

This little equation is all we need for balancing the pendulum near the top equilibrium. $K$ is six numbers, one per state variable, computed once from $P$ by plain matrix multiplication. From then on the controller just measures the six state values, multiplies each by its matching number in $K$, adds them up, negates the total, and that's how hard it pushes. Conceptually, the entire balance controller is just a dot product: Multiply two vectors and get one scalar result. Here: each state value times its matching gain, summed into one force command. running at 600Hz (10 substeps per frame):

How Far Can It Lean and Still Come Back?

LQR is optimal for linearized systems, but the machine is only roughly linear near the top equilibrium. The further it gets from upright, the worse the approximation becomes, until the controller is solving a problem that no longer describes reality. The set of states from which it can actually recover is its basin of attraction: The full set of starting states from which a given controller eventually settles at its goal. A controller keeps computing the same way outside this set, it just no longer converges anywhere useful. .

If the pendulum links lean in the same direction (like a single rigid stick) that's a standard tilt and catching it is like the simple broom example from earlier. If the two links go in opposite directions (buckling into a zig-zag) it becomes a lot more complicated to correct.

Leaning both links in the same direction, it survives out to about 20°. But fold them in opposite directions and the boundary: Where a basin of attraction stops. On one side, every starting state converges; cross over and none of them do, often quite abruptly. drops to roughly 12°.

Planning the Swing

The linear balance controller only works when the links are within about 20° of upright. If the pendulum starts out hanging straight down at 180°, LQR can't be used to make it reach the balanced state at the top. That is because getting upright requires swinging backward first to build momentum, deliberately making your error bigger before making it smaller.

So now the objective has to be reframed. Instead of "what force should I apply right now?" it becomes "what is the whole sequence of forces, over the next X seconds, that I need to apply so that the pendulum ends up at upright equilibrium?" Generally, this is trajectory optimization: Searching for a whole sequence of control inputs over time, rather than an instantaneous rule. It can represent plans that get worse before they get better, which no reactive controller can. .

iLQR for Trajectory Optimization

Control engineers will learn through their schooling/textbooks a sort of flowchart that they can use to help them decide what controller they should use to solve a given control problem. I built an interactive controller picker if you want to see an actual flowchart for this purpose.

The double pendulum is a classic problem, so the standard choices for how to control it are well known. To save you the hassle let me just tell you, near the top we use LQR (as we've already discussed), and when we get out of that 20° range we use a trajectory optimization method called iterative LQR (iLQR): A trajectory optimization algorithm that finds a sequence of control inputs by repeatedly linearizing the physics along the current guess, solving that easier linear version exactly, and refining the guess. The 'i' is for iterative. . The idea behind iLQR is simple. Take some guess at a trajectory, however bad, linearize the physics along that guess, solve that easy version, take the new, better guess, then linearize along it and do it again.

So now that we know how we will plan the trajectory, it's time for another LQR variant to be introduced: TVLQR: Time-Varying Linear Quadratic Regulator. Instead of one fixed gain matrix near upright, TVLQR computes a schedule of gains K(t) along an entire planned trajectory, adjusting how hard it corrects at every point in time. . A trajectory on its own is just a plan. Without feedback, the smallest nudge will ruin that plan. To actually follow the path, you need gains that adjust at every point in time along the swing. This is what TVLQR is used for here.

You can compute TVLQR along any trajectory but pairing TVLQR with iLQR has a big benefit: we get the TVLQR gain schedule basically for free. Let me explain. Every time iLQR takes an iteration, it runs a backward pass: The sweep that runs from the end of the trajectory back to the beginning. It has to go that way — the best thing to do now depends on what happens afterwards, so the future gets solved first. from the end of the trajectory back to the start. At each step $t$ along the trajectory (one every 20 ms), that backward pass solves for the optimal control adjustment:

It computes two things at every step. First is $k_t$, which is the change in planned motor force at time t to move the swing closer to the optimal path. Second is $K_t$, which is the feedback gain that says how to correct if we are not where we expected to be.

Once iLQR converges, the $k_t$ adjustments shrink to zero since the converged plan can't be improved any further. However, the feedback matrices $K_t$ stick around. The backward pass found them by linearizing the physics and solving a Riccati recursion along the trajectory, which is exactly what TVLQR does, so the $K_t$ matrices from the final iteration are the TVLQR gains. That's how it's "free", we get the path to follow as well as the gains to correct against deviations from that path all in one go. Pretty neat, huh?

In an ideal world, we’d simply take those $k_t$ nudges, apply them to the motor inputs, and roll out a new, improved trajectory. But there’s a catch (why is there always a catch 😔). Just like the 20° balancing window earlier, this linearized model is only a local approximation. If the solver takes too big of a step it might escape the region where the linearization is valid and result in an overall worse action for achieving the balancing goal.

To keep the loop in Fig. 4 from flying off the rails, there are two safeguards. First, during the backward pass, the step is regularized: Adding a damping term to a step calculation so it doesn't overreact when the linearized model is inaccurate. . This adds a damping term, just like in Levenberg–Marquardt: A standard trick in nonlinear optimization where you blend a bold Newton step with a cautious downhill step, leaning on the cautious one whenever the local model looks untrustworthy. , so when the simplified physics causes the solver to take an optimization step that looks untrustworthy, the solver takes a baby step downhill instead of a giant leap down the hill.

Second, the solver runs a backtracking line search (Step 3 in the loop). It tests the full 100% correction first. If this actually reduces the total cost in the nonlinear simulation, great! If not, it tries 80%, then 60%, and so on until it finds a step that improves things. With those two guardrails in place, the solver guarantees non-increasing cost on each iteration without blowing up.

To actually put this into practice for the double pendulum we need to solve the balancing problem in four stages. In stage one, we set a very loose penalty on missing the upright target, just looking for any rough motion that gets the linkage swinging high. Once it discovers the gross swing, we crank up the target penalty in stages, forcing the solver to clean up the trajectory and stick the landing. If we demand perfection too early, the solver doesn't like the penalties and it won't be brave enough to commit to a big swing that it'll need to get the links upright.

What It Found

Once iteration 566 wraps up, the resulting plan is tidy. I want to remind you here that the optimizer was not specifically told to pump. It was just given a model of the physics, a starting pose, a target pose, and a mild penalty for using excessive force, and it worked out the wind-up on its own because that's the cheapest way to satisfy the constraints.

The energy numbers in the graph make it clear what the swing-up is actually for. Hanging at rest, the machine sits at $-19.62\text{J}$. Balanced at rest, it needs $+19.62\text{J}$, same magnitude, opposite sign, which I hope is obvious. The swing-up is moving 39.24 joules into the system through its motor force.

Putting the Plan into Action

Now that we have the planned trajectory from Fig. 6, it’s tempting to think the hard work is done. But we can't just press play and replay that sequence of forces.

Because the double pendulum is exponentially unstable, open-loop replay just won't work. Any little discrepancy doubles and doubles again very quickly. In almost every conceivable scenario this makes the cart add chaos instead of achieving equilibrium.

This is where those "free" TVLQR feedback gains $K(t)$ I discussed earlier add value. Instead of treating the trajectory as a rigid script, the controller uses it as a moving reference and nudges the motor with those feedback gains whenever the machine drifts off course.

One Plan, Many Starting Poses

This trajectory following approach might bring about a question for you (it did for me). If there's only one stored trajectory, solved from one specific starting pose, how does that single plan cover all of the possible poses the machine might be in when you start it?

The controller doesn't solve this problem by having more plans. It works as-is because the correction term actively drags nearby states onto the plan. The nominal trajectory acts like a magnetic line, and the gains are what pull nearby states toward it. You might also hear this described as a "funnel" in control theory textbooks but for me the magnet analogy is more intuitive.

This "magnetism" has a limit, just like an actual magnet. The runs above are perturbed by up to about 7° in each link and a quarter of a radian per second in each spin rate. Push much beyond that and the correction the gains require exceeds what the motor can deliver and the swing-up fails.

The Hand-Over

Now I've told you about the two controllers needed to solve this balancing problem. The TVLQR controller that tracks the swing-up trajectory that the iLQR optimization found, and the LQR controller that balances the pendulum near upright. I'll now explain how to switch between them smoothly.

If you simply hard switch from one to the other somewhere near the top, their control commands will almost certainly disagree. That then causes a sudden jolt and can be enough to knock it straight back down. In control theory, switching modes without causing sudden spikes in actuator effort is known as a bumpless transfer: A control technique that prevents abrupt changes in the controller output when switching between operational modes. . Often this requires delicate blending filters or threshold tuning, but for this problem, the textbook solution simply uses boundary conditions.

To compute the swing-up gains, the backward pass starts at the finish line (t = 4s) and solves backwards to the start. But before it can take its first step, it needs an answer to a simple question: if the pendulum arrives at the top tilted by even a fraction of a degree, how much penalty should that carry?

After the swing-up is completed, the linear balance controller takes over. Back in The Balancing Act section, the Riccati equation gave us a matrix, $P$, which is the price sheet for how much future effort it costs to balance out any tilt with that controller. This is the lazy (or brilliant?) solution. We simply re-use that exact $P$ matrix as the starting penalty at t = 4s for the backpass math. Because both calculations use the same cost and physics at the finish line, once the recursion is substepped, the swing-up’s final gain numbers match the balance controller's numbers.

There’s no switching logic or mode-blending needed. At four seconds, the swing-up controller naturally morphs into the balance controller. You can see this in Fig. 9: while the solid lines (the TVLQR swing-up gains) swing wildly early on, as the clock approaches four seconds they settle directly onto the dashed lines (the steady-state LQR balance controller).

Coming Back Down

If you knock the pendulum over, it's got to have a way to get back up. But its reset has to be actively controlled, because a pendulum that's just lost its balance can swing around pretty chaotically. Energy enters and leaves the machine at a rate equal to force times cart velocity, u·v. To remove energy, use a force that opposes the cart's motion, u = −k·v, which drains energy at k·v².

Crank k up too high and the cart locks in place: v drops to zero, and energy stops leaving the links. The cart needs to stay movable to act as a damper so the pendulum can actually dump its energy into it.

Teaching an Old Dog The Same Trick

So how do the experts solve this? If we use Russ Tedrake's Underactuated Robotics notes, you've already learned the answer. We just use LQR again.

LQR is not only for precarious balancing acts at the top. LQR works around any equilibrium fixed point where the forces cancel. Hanging straight down (θ = ±π) is an equilibrium just like balancing upright (θ = 0). All the math from before is the same, we just solve the Riccati equation linearized about the hanging pose.

The equation and solver are identical to the balancing case, but the tuning is flipped. For balancing upright, we penalized link tilt aggressively (Q_θ = 150) and tolerated cart drift. For braking, gravity stabilizes the angles for free. We dial the angle penalties down to 0.5 and crank the cart position penalty up to 50. That keeps the cart centered on the rail while letting it "dance" with the swing, timing horizontal moves to dump kinetic energy out of the links as fast as possible.

The Numbers Breakdown

Because nerds like me love numbers, let's go through the numbers of this writeup. I ran 50 iLQR solves (five horizons, five random starting guesses, two noise levels) and kept the best one which took 70 seconds in total. The winning iLQR solve took 566 iterations, each one linearizing the physics along 200 knots, taking 1.8 seconds on my Macbook M3 Pro. It produced 2,606 numbers and occupies 21 KB as a static table. Everything you've been seeing run live in the figures above is just evaluating $u=u_{\text{plan}}-K(t)(x-x_{\text{plan}})$, which is 6 multiplications and an interpolation on every physics step. Because the heavy part was done offline, the live controller can run at 600Hz (10 substeps per frame). I can't render the animations at 600Hz but I can hit a smooth 60fps with ease.

A lot of real robots split the work the same way, heavy planning offline or at a few hertz, light feedback in a tight loop. The browser is running the part a real machine's control loop would actually run.

Appendix: Gotchas That Got Me

I wanted to just add a few notes here about the things I got wrong when I was implementing this code and learning the theory. These are some of the practical things that you learn by doing, not just reading about "how to solve it". Maybe my mistakes will save you some time if you try to implement this, or something similar, yourself.

• TVLQR tracking gains along the planned iLQR path need the timestep folded in. When computing the TVLQR feedback gains along the nominal iLQR trajectory, continuous-time cost weights need the timestep multiplied in (Q·Δt). I forgot this the first time at 50 Hz (Δt = 0.02s), which left the gains 50× too small and made the controller look completely dead. On top of that, a 20 ms knot interval was too coarse for how fast the double pendulum falls, distorting the gains by ~20%. Substepping: Running the recursion on a finer internal time grid than the one it stores results at — several small steps between each knot instead of one big one, for better accuracy without changing what gets saved. the backward Riccati pass inside each knot fixed it.
• Anything periodic needs its errors wrapped: Forcing an angle back into a single canonical range, usually (−π, π], by adding or subtracting full turns until it lands there. Without it, two physically identical angles (like +π and −π) look numerically far apart. . +π and −π are the exact same physical pose, but 6.28 apart numerically. I lost an embarrassing amount of time to one unwrapped comparison buried three functions deep, which caused the controller to spin a link 360° to fix an error that didn't exist. The fix was a simple wrapping helper, wrapPi(θ), that maps all angular errors back into (−π, π] before they ever hit a gain multiplication.
• Validate twice: once in the offline solver, once in the live sim. The offline optimizer runs in double precision with a fine RK4 integrator. The browser runs in a 60 fps requestAnimationFrame loop against gains rounded to three decimals. At first, taking a single Euler step per animation frame accumulated enough numerical drift that the live sim missed the balance catch entirely. I fixed this by substepping the browser physics (taking 10 small integration steps per frame, running physics at 600 Hz) and adding an automated validation script that verifies the exact rounded numbers under browser dynamics before shipping.