burnboom/mujoco-triple-swingup

Triple pendulum swing-up on a cart (MuJoCo)

Three linked poles on a cart, one motor, swung up from hanging and balanced. Self-contained: one Python file, no neural network, no GPU.

This is a classical controller, not a learned model. It is published here because the artefacts are reusable: a solved swing-up trajectory, its tracking gain schedule, and a script that regenerates both from scratch in under a second.

Results

Measured in MuJoCo, 24 independent trials with randomised initial perturbation:

Metric	Value
Trajectory solve time (IPOPT)	0.29 s
Caught and held	24 / 24 (100%)
Worst final angle	0.04 deg
Peak motor force used	36.6 N of a 60 N limit
Swing-up duration	3.02 s

Robustness to the initial perturbation, 64 episodes per row:

Initial noise	Caught
0.00 rad	100%
0.01 rad (0.57 deg)	100%
0.02 rad (1.15 deg)	100%
0.04 rad (2.29 deg)	84%

Usage

Requires mujoco, numpy and casadi only.

pip install mujoco numpy casadi
python triple_swingup_standalone.py          # headless, prints the three stages
python triple_swingup_standalone.py --view   # opens the MuJoCo viewer

To use the precomputed artefacts instead of re-solving:

import numpy as np

gains = np.load("swingup3_gains.npz")
q_rel, qd_rel = gains["q_rel"], gains["qd_rel"]   # nominal, MuJoCo joint coords
u = gains["u"]                                     # nominal forces, N
K = gains["K"]                                     # [151, 1, 8] tracking gains
K_catch = gains["K_catch"]                         # [8] balancing LQR gain
dt = float(gains["dt"])                            # 0.02 s control step

# while the plan runs:   u_k = u[k] - K[k] @ (x - x_nom[k])
# after it ends:         u   = -K_catch @ x

How it works

A feedback law cannot do this. Energy shaping solves the single pendulum swing-up because dE/dt = -m*l*a*thd*cos(th) gives one scalar to pump; with three links there is no such handle, and the balancing LQR's region of attraction is roughly 2 degrees per link, so nothing lands in it by chance.

1. Plan. Direct collocation via IPOPT. Positions, velocities, accelerations and forces are all decision variables; the dynamics enter as local equality constraints, so the Jacobian is sparse and there is no long chain to differentiate through. Accelerations being variables (with D(q)·a - rhs(q, qd, u) = 0 as the constraint) keeps everything linear in a and avoids a symbolic matrix inverse.

2. Track. A triple pendulum is chaotic, so the plan is useless open loop. MuJoCo's own transition is linearised about each knot with mjd_transitionFD, and the Riccati recursion is swept backwards from the balancing controller's cost-to-go to give a gain schedule.

3. Catch. At the end of the plan, hand over to an infinite-horizon LQR about upright.

Gotchas worth knowing

These each cost real debugging time and are non-obvious:

• Open-loop replay does not work. Replaying the nominal forces from the exact planned initial state ends at -520, -397, -784 degrees. The system is chaotic; the plan must be tracked, never replayed.
• Plan with the simulator's real inertia, not textbook rod formulas. MuJoCo builds inertia from the capsule geometry, whose end caps make it about 1% larger than an ideal thin rod. Planning with rod inertia made the tracker diverge exponentially (0.7 deg at 0.6 s, 23.7 deg at 1.5 s) and saturate the motor at 0% success. Reading the constants off the compiled model fixed it outright.
• Gradient descent on a shooting objective does not solve this. Before collocation, single shooting with 128 random restarts plateaued at 17.5 degrees against the ~3 needed, sitting at exactly the same cost for 250 iterations. Gravity continuation made it worse, not better. It is not an energy or actuator limit: raising all three links costs 14.1 J and the solver never uses more than 36 of its 60 N.
• mjd_transitionFD rejects the RK4 integrator. Use implicitfast at a finer step and compose the per-substep linearisation up to the control rate as A = As^n, B = (I + As + ... + As^(n-1))·Bs, which is exact for a zero-order hold.
• Watch for spurious contacts. The cart box encloses the rail capsule. If they are collidable, MuJoCo generates contacts that damp the cart and swallow the motor force, producing LQR gains around 1e4 and an expert that falls over immediately. contype=0 conaffinity=0 throughout.

Files

File	Contents
triple_swingup_standalone.py	The whole thing: model, planner, tracker, runner
swingup3_collocated.npz	Nominal trajectory: q, qd, u (151 knots, dt 0.02 s)
swingup3_gains.npz	Tracking gains K [151,1,8], K_catch [8], nominal in MuJoCo joint coordinates
triple_swingup_balanced.png	Rendered result

Model

Cart 1.5 kg on a rail; three 0.4 m / 0.4 kg poles; motor limited to +/-60 N; cart travel limited to +/-2.0 m during planning. Control at 50 Hz, physics at 200 Hz with implicitfast.

Notes

Swing-up of a triple pendulum on a cart is an established result in the optimal-control literature, with hardware validation published by Glueck, Eder and Kugi (Automatica, 2013). This repository is an independent MuJoCo implementation of that general architecture — offline trajectory optimisation plus feedback stabilisation — not a reproduction of their specific method or parameters.