Double Pendulum Balancing Robot | Jibrail Bhagwandass

Python LQR Control Swing-Up Control SymPy SciPy NumPy Lagrangian Mechanics RK4 Integrator Matplotlib YAML Config

Overview

Swinging Up and Balancing a Chaotic System

The double pendulum is one of the canonical problems in nonlinear dynamics and control theory. Unlike a single inverted pendulum, it is underactuated — one actuated degree of freedom (the cart) must control two unactuated joints — and exhibits chaotic behaviour, making stabilisation significantly harder.

This project builds a physics-accurate 2D simulation from first principles and implements a two-phase controller: an Åström-Furuta energy-pumping swing-up followed by seamless handoff to an LQR stabiliser at the upright unstable equilibrium. All controller parameters are analytically derived from the system physics — no manual gain tuning.

System Configuration

          ● tip (link 2)
         /
        /  link 2  (m₂ = 0.25 kg, l₂ = 0.5 m)
       ●  joint
      /
     /  link 1  (m₁ = 1.5 kg, l₁ = 1.5 m)
    ●  pivot (attached to cart)
  [====]  cart  (M = 1.0 kg)
──────────────────────────  track
      → u  (horizontal control force — only actuated DOF)

State vector:  [ x,  θ₁,  θ₂,  ẋ,  θ̇₁,  θ̇₂ ]

Performance

Results Against Formal Requirements

Metric	Result	Requirement	Status
Swing-up + LQR capture	< 2 s	≤ 15 s (FR-04)	✓ PASS
Settled within ±5°	~15 s	≤ 15 s (FR-04)	✓ PASS
θ₁ max error (last 30 s)	0.056°	≤ 5° (FR-05)	✓ PASS
θ₂ max error (last 30 s)	0.002°	≤ 5° (FR-05)	✓ PASS
Energy drift (5 s free-swing)	0.0000%	Numerically stable (NFR-02)	✓ PASS
Simulation update rate	200 Hz	≥ 100 Hz (FR-02)	✓ PASS

Key Engineering Features

Full Lagrangian equations of motion derived symbolically with SymPy — mass matrix M(q), Coriolis C(q,q̇), and gravity G(q) — then lambdified for fast numerical runtime evaluation
RK4 integrator at 200 Hz — energy drift under 0.001% over 30 seconds, confirming numerical stability
LQR stabiliser — continuous algebraic Riccati equation solved via scipy.linalg.solve_continuous_are; linearisation via numerical finite differences at the upright equilibrium
Åström-Furuta swing-up — energy-pumping controller drives pendulum-only mechanical energy toward the upright target; gains auto-derived analytically from system parameters
Force-gate LQR capture — prevents premature handoff at high-velocity passes where required restoring force would immediately diverge
Auto-tuner (tune.py) derives all controller parameters analytically from physics and validates against FR-04/FR-05 with adaptive refinement
Real-time interactive visualiser — mouse-drag cart, press G to trigger swing-up, Space to pause
All physical parameters externally configurable via YAML config — no hardcoded constants

Technical Approach

Two-Phase Controller Design

Phase 1 — Swing-Up (Åström-Furuta)

Drives pendulum-only energy toward upright target E_target
Control law: u = k_sw · ẋ · (E_target − E_pend)
Initial kick (~0.09 s) breaks the hanging dead-point equilibrium
All gains derived analytically: u_max = 4·(M+m₁+m₂)·g

Phase 2 — LQR Stabilisation

Linearises nonlinear EOM at upright equilibrium (θ₁, θ₂) = (0, 0)
Solves continuous CARE for optimal gain matrix K
Control law: u = −K · state
Cart position x left unpenalised — cart translates freely while links balance

The handoff from swing-up to LQR uses a force-gate condition: capture occurs only when both |θ₁|, |θ₂| ≤ 30° AND the predicted LQR restoring force is ≤ 3 · u_max. This prevents the LQR from taking over during a high-speed pass through upright — a subtle failure mode that would cause immediate divergence.

# Swing-up energy pump (simplified)
E_pend = compute_pendulum_energy(state, params)
u = k_sw * state[3] * (E_target - E_pend)    # ẋ × energy error
u = np.clip(u, -u_max, u_max)

# Force-gate: only hand off to LQR when near upright AND force is manageable
if abs(theta1) < capture_angle and abs(theta2) < capture_angle:
    lqr_force = -K @ state
    if abs(lqr_force) <= 3 * u_max:
        mode = 'lqr'

Architecture

Modular Codebase

The project is structured as fully independent modules — each with its own unit tests — and a single main.py entry point that wires them together. All physical constants live in config.yaml; running update_config.py recomputes derived quantities (moments of inertia, sample rate) when parameters change.

Modules

dynamics.py — Symbolic EOM, RK4 integrator
controller_lqr.py — LQR & SwingUpLQR controllers
visualizer.py — Real-time 2D animation
logger.py — CSV/JSON state logging
tune.py — Analytical auto-tuner
report.py — Full performance report

Interactive Controls

Left-click + drag — move cart to cursor
G / Enter — trigger swing-up → LQR
R / Right-click — reset to hanging at rest
Space — pause / resume simulation

Tech Stack

Tools & Libraries

Core

Python 3.11+
NumPy — numerical arrays & linear algebra
SciPy — ODE solver, CARE solver
SymPy — symbolic EOM derivation
Matplotlib — real-time 2D animation

Supporting

PyYAML — external parameter config
pytest — unit & integration tests
CSV / JSON — trajectory logging
RK4 — custom integrator (200 Hz)

Simulation Output

Live Run — Swing-Up & Energy Plot

Real-time output from a swing-up run showing the cart, both pendulum links, and the mechanical energy breakdown (kinetic, potential, total) confirming near-perfect energy conservation throughout.

Double Pendulum Balancing Robot — Simulation