Cart-Pole Limit Cycle via Symbolic Lagrangian Mechanics

This tutorial demonstrates how to combine symbolic derivation of equations of motion (via Symbolics.jl) with direct optimal control (via OptimalControl.jl) to find a periodic orbit (limit cycle) for a cart-pole system.

The key is to define the kinetic and potential energies and the power of the non-conservative forces, and to let Symbolics.jl handle the derivations of the equations of motion using the Lagrange-Euler equation.

The Cart-Pole System

The system consists of a cart of mass $m_c$ sliding on a frictionless horizontal rail, with a rigid pendulum of mass $m_p$ and length $l$ attached to it. A horizontal force $u$ (the control input) acts on the cart.

The configuration vector is $q = (x,\, \theta)$, where $x$ is the cart position and $\theta = 0$ corresponds to the upright (unstable) equilibrium of the pendulum.

Positions

The Cartesian positions of the two bodies are:

\[p_c = \begin{pmatrix} x \\ 0 \end{pmatrix}, \qquad p_p = \begin{pmatrix} x + l\sin\theta \\ l\cos\theta \end{pmatrix}.\]

Lagrangian

The kinetic and potential energies are:

\[T = \tfrac{1}{2}m_c\,\|\dot{p}_c\|^2 + \tfrac{1}{2}m_p\,\|\dot{p}_p\|^2 = \tfrac{1}{2}(m_c+m_p)\dot{x}^2 + m_p l\,\dot{x}\dot{\theta}\cos\theta + \tfrac{1}{2}m_p l^2\dot{\theta}^2,\]

\[V = m_p\,g\,l\cos\theta.\]

The Lagrangian is $\mathcal{L} = T - V$. The virtual power $P_{nc}$ of the non-conservative force (control) $F = (u, 0)$ acting on the cart gives the generalised force vector:

\[P_{nc} = F \cdot \dot{p}_c = u\,\dot{x}, \quad\Longrightarrow\quad Q = \frac{\partial P_{nc}}{\partial \dot{q}} = \begin{pmatrix} u \\ 0 \end{pmatrix}.\]

Euler–Lagrange Equation

The equations of motion follow from:

\[\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}} - \frac{\partial \mathcal{L}}{\partial q} = Q.\]

They can be written in the standard manipulator form (also known as robotic equation of motion):

\[M(q)\,\ddot{q} = -C(q,\dot{q}) + \tau(q, u),\]

where the symmetric positive-definite mass matrix is:

\[M(q) = \begin{pmatrix} m_c + m_p & m_p l \cos\theta \\ m_p l \cos\theta & m_p l^2 \end{pmatrix},\]

and the right-hand side collects Coriolis/gravity terms and the control torque. Instead of deriving these by hand, we rely on Symbolics.jl to compute and analytically invert $M(q)$ for us.

State-Space Form

Defining the state $X = (x,\,\theta,\,\dot{x},\,\dot{\theta})$, the equations of motion become the first-order system:

\[\dot{X}(t) = f\!\left(X(t),\, u(t)\right) = \begin{pmatrix} \dot{x} \\ \dot{\theta} \\ M^{-1}(q)\bigl(-C(q,\dot{q}) + \tau(q,u)\bigr) \end{pmatrix}.\]

Optimal Control of a Cart-Pole System using Symbolics.jl

This tutorial demonstrates how to use Symbolics.jl to automate the derivation of equations of motion (EOM) for a mechanical system and subsequently solve an optimal control problem using OptimalControl.jl.

Implementation

Setup & Imports

using OptimalControl
using NLPModelsIpopt
using Symbolics
using LinearAlgebra: dot
using Plots

Physical Parameters and Symbolic Variables

We declare all parameters both as numerical constants (for the final function evaluation) and as symbolic variables (for the Lagrangian computation). We define the configuration vector $q = (x, \theta)$.

# Physical constants
const m_c_val = 5.0
const m_p_val = 1.0
const l_val   = 2.0
const g_val   = 9.81
const tf_val  = 2.0

# Symbolic variables
@variables t
D = Differential(t)
@variables m_c m_p l g u
@variables x(t) θ(t)

q = [x, θ]

Automated Kinematics and Lagrangian

We express the positions, kinetic energy, potential energy, and (non-conservative) power symbolically. The time derivatives $\dot{p}_c$ and $\dot{p}_p$ are computed automatically by D.(...).

p_c = [x, 0.0]
p_p = [x + l * sin(θ), l * cos(θ)]
F_ext = [u, 0.0]

T = 0.5 * m_c * sum(abs2, D.(p_c)) + 0.5 * m_p * sum(abs2, D.(p_p))
V = g * (m_p * p_p[2])
L = T - V

P_non_conservative = dot(D.(p_c), F_ext)

Euler–Lagrange Equations and Mass-Matrix Inversion

Starting from $\mathcal{L} = T - V$, Symbolics.jl computes the terms of the Euler–Lagrange equations. To isolate the accelerations, we substitute the symbolic time derivatives with algebraic variables. This allows us to identify the standard manipulator form components: the mass matrix is the Jacobian of the residual with respect to the accelerations $\ddot{q}$, and the bias vector contains the remaining terms.

A = D.(Symbolics.gradient(L, D.(q)))  # d/dt(∂L/∂q̇)
B = Symbolics.gradient(L, q)          # ∂L/∂q
Q = Symbolics.gradient(P_non_conservative, D.(q))

# Euler-Lagrange residual: d/dt(∂L/∂q̇) - ∂L/∂q - Q = 0
el_eqs = expand_derivatives.(A - B - Q)

# Helper to create algebraic variables for derivatives
diff_var(qi, suffix) = Symbolics.variable(Symbol(Symbolics.operation(Symbolics.value(qi)), suffix))

# Static aliases for velocities and accelerations
v = diff_var.(q, :_t)
dv = diff_var.(q, :_tt)

# Freeze time derivatives into static algebraic variables
sub_rules = Dict([D.(q) .=> v; D.(D.(q)) .=> dv])
residual = Symbolics.substitute.(el_eqs, (sub_rules,))

# Identify mass matrix M and bias vector b such that M·dv + b = 0
mass = Symbolics.jacobian(residual, dv)
bias = Symbolics.substitute.(residual, (Dict(dv .=> 0.0),))

# Solve for accelerations analytically: dv = M⁻¹(-b)
accel = Symbolics.simplify_fractions.(mass \ (-bias))

# Fully explicit state derivative: Ẋ = [v, accel]
X = [q; v]
dX = [v; accel]

Code Generation

build_function compiles the symbolic expression dX into a native Julia function with arguments X, u, and parameter values. The force_SA=true flag generates a StaticArrays kernel, which avoids heap allocations inside the ODE right-hand side — crucial for solver performance because dimension is small. For larger problems ($X \in \mathrm{R}^n$, $n > 100$), we would use a mutating dynamics function instead, cf. Julia Performance Tips.

f_expr = build_function(dX, X, u, [m_c, m_p, l, g];
    expression=Val{false}, force_SA=true)
f_sym = f_expr[1]   # out-of-place variant: (state, u, params) → SVector

const p_vals  = [m_c_val, m_p_val, l_val, g_val]

cartpole_dynamics(X, u) = f_sym(X, u, p_vals)

Optimal Control Problem Definition

We now formulate the optimal control problem using the @def macro from OptimalControl.jl. The initial state is not at rest because $ω(0) = 0.2$, while the boundary condition $X(0) - X(tf) = 0$ encodes the periodicity of the orbit.

@def cartpole begin
    t ∈ [0, tf_val], time
    X = (x, θ, v, ω) ∈ R⁴, state
    u ∈ R, control

    x(0) == 0
    θ(0) == 0
    v(0) == 0
    ω(0) == 0.2
    X(tf_val) - X(0) == [0, 0, 0, 0]  # Periodic orbit

    Ẋ(t) == cartpole_dynamics(X(t), u(t))

    ∫(u(t)^2) → min
end

Abstract definition:

    t ∈ [0, tf_val], time
    X = ((x, θ, v, ω) ∈ R⁴, state)
    u ∈ R, control
    x(0) == 0
    θ(0) == 0
    v(0) == 0
    ω(0) == 0.2
    X(tf_val) - X(0) == [0, 0, 0, 0]
    Ẋ(t) == cartpole_dynamics(X(t), u(t))
    ∫(u(t) ^ 2) → min

The (autonomous) optimal control problem is of the form:

    minimize  J(X, u) = ∫ f⁰(X(t), u(t)) dt, over [0, 2.0]

    subject to

        Ẋ(t) = f(X(t), u(t)), t in [0, 2.0] a.e.,

        ϕ₋ ≤ ϕ(X(0), X(2.0)) ≤ ϕ₊, 

    where X(t) = (x(t), θ(t), v(t), ω(t)) ∈ R⁴ and u(t) ∈ R.

Solving the NLP

The problem is transcribed into a nonlinear program using direct collocation on a uniform grid of 100 intervals, then handed to Ipopt via NLPModelsIpopt.jl. We provide a simple initial guess for the state and control trajectories. See the documentation for more information.

initial_guess = @init cartpole begin
    X(t) := [0.0, 0.0, 0.0, 0.2]
    u(t) := 0.0
end

sol = solve(cartpole; display=false, grid_size=100, init=initial_guess)

• Solver:
  ✓ Successful  : true
  │  Status     : first_order
  │  Message    : Ipopt/generic
  │  Iterations : 3
  │  Objective  : 1319.790109169773
  └─ Constraints violation : 6.676326158583379e-13

• Boundary duals: [2.0076425910138758e-13, -1.015676502361834e-11, -4990.076339128379, -12844.887616517452, 2495.038169564187, 14667.425629531566, -2495.0381695641886, -6422.443808258724]

Results

tsol = time_grid(sol)
Xsol = state(sol).(tsol)
usol = control(sol).(tsol)

X_mat = reduce(hcat, Xsol)
q_sol = X_mat[1:2, :]'
dq_sol = X_mat[3:4, :]'

p1 = plot(tsol, q_sol, label=["x" "θ"], title="Configuration")
p2 = plot(tsol, dq_sol, label=["v" "ω"], title="Velocities")
p3 = plot(tsol, usol, label="u", title="Control", linetype=:steppost)

plot(p1, p2, p3, layout=(3, 1), size=(800, 700))

The plots show the cart position $x$ and pendulum angle $\theta$, the corresponding velocities, and the optimal control force $u$ required to stabilize the system back to its initial state within 2 seconds.

Animation

The animation below shows the cart-pole evolving along the optimal limit-cycle trajectory. The blue cart slides on the horizontal rail while the pendulum swings around the upright equilibrium.