Cartpole Swingup#

Mathematical Formulation#

Problem Statement#

Find the optimal control force \(F(t)\) that swings up an inverted pendulum (cartpole) from the downward stable position to the upward unstable position in minimum time:

\[J = t_f\]

Subject to the coupled cart-pole dynamics:

\[\frac{dx}{dt} = \dot{x}\]
\[\frac{d\theta}{dt} = \dot{\theta}\]
\[\frac{d\dot{x}}{dt} = \frac{4F + 3gm\sin(2\theta)/2 - 2lm\sin(\theta)\dot{\theta}^2}{4M + 3m\sin^2(\theta) + m}\]
\[\frac{d\dot{\theta}}{dt} = \frac{3[2g(M + m)\sin(\theta) + (2F - lm\sin(\theta)\dot{\theta}^2)\cos(\theta)]}{l(4M + 3m\sin^2(\theta) + m)}\]

Boundary Conditions#

  • Initial conditions: \(x(0) = 0\) m, \(\theta(0) = \pi\) rad, \(\dot{x}(0) = 0\) m/s, \(\dot{\theta}(0) = 0\) rad/s

  • Final conditions: \(x(t_f) = 0\) m, \(\theta(t_f) = 0\) rad, \(\dot{x}(t_f) = 0\) m/s, \(\dot{\theta}(t_f) = 0\) rad/s

  • Control bounds: \(-10.0 \leq F \leq 10.0\) N

Physical Parameters#

  • Cart mass: \(M = 1.0\) kg

  • Pole mass: \(m = 0.1\) kg

  • Pole length: \(l = 0.5\) m

  • Gravity: \(g = 9.81\) m/s²

State Variables#

  • \(x(t)\): Cart position (m)

  • \(\theta(t)\): Pole angle from upright vertical (rad)

  • \(\dot{x}(t)\): Cart velocity (m/s)

  • \(\dot{\theta}(t)\): Pole angular velocity (rad/s)

Control Variable#

  • \(F(t)\): Horizontal force applied to cart (N)

Notes#

This is a classic underactuated control problem where the cart-pole system must swing up from the natural downward equilibrium (\(\theta = \pi\)) to the unstable upward equilibrium (\(\theta = 0\)) using only horizontal forces on the cart. The coupling between cart motion and pole dynamics enables the swingup maneuver.

Dynamics Derivation#

The cart-pole dynamics were derived using Lagrangian mechanics with SymPy. The derivation handles the coupled cart-pole system with external force input:

examples/cartpole_swingup/cartpole_swingup_dynamics.py#
  1import sympy as sm
  2import sympy.physics.mechanics as me
  3
  4from maptor.mechanics import lagrangian_to_maptor_dynamics
  5
  6
  7# ============================================================================
  8# Physical Parameters
  9# ============================================================================
 10
 11M, m, l, g = sm.symbols("M m l g")  # Cart mass, pole mass, pole length, gravity
 12F = sm.symbols("F")  # Applied force on cart
 13x, theta = me.dynamicsymbols("x theta")  # Cart position, pole angle from vertical
 14xd, thetad = me.dynamicsymbols("x theta", 1)  # First derivatives
 15
 16
 17# ============================================================================
 18# Reference Frames
 19# ============================================================================
 20
 21N = me.ReferenceFrame("N")  # Inertial frame
 22B = N.orientnew("B", "Axis", (theta, N.z))  # Pole body frame
 23
 24
 25# ============================================================================
 26# Points and Velocities
 27# ============================================================================
 28
 29O = me.Point("O")  # Fixed origin
 30O.set_vel(N, 0)
 31
 32# Cart center of mass
 33cart_point = O.locatenew("cart", x * N.x)
 34cart_point.set_vel(N, xd * N.x)
 35
 36# Pole center of mass (l/2 from cart along pole)
 37pole_point = cart_point.locatenew("pole", (l / 2) * B.y)
 38pole_point.v2pt_theory(cart_point, N, B)
 39
 40
 41# ============================================================================
 42# Inertia and Rigid Bodies
 43# ============================================================================
 44
 45# Cart: point mass (no rotational inertia about center)
 46I_cart = me.inertia(N, 0, 0, 0)
 47cart_body = me.RigidBody("cart", cart_point, N, M, (I_cart, cart_point))
 48
 49# Pole: uniform rod inertia about center of mass
 50I_pole_cm = (m * l**2 / 12) * me.inertia(B, 0, 0, 1)
 51pole_body = me.RigidBody("pole", pole_point, B, m, (I_pole_cm, pole_point))
 52
 53
 54# ============================================================================
 55# Forces (Passive Only)
 56# ============================================================================
 57
 58loads = [
 59    (cart_point, -M * g * N.y),  # Gravity on cart
 60    (pole_point, -m * g * N.y),  # Gravity on pole
 61]
 62
 63
 64# ============================================================================
 65# Lagrangian Mechanics
 66# ============================================================================
 67
 68L = me.Lagrangian(N, cart_body, pole_body)
 69LM = me.LagrangesMethod(L, [x, theta], forcelist=loads, frame=N)
 70
 71
 72# ============================================================================
 73# Control Forces
 74# ============================================================================
 75
 76# F acts on x coordinate (cart position), no direct force on theta coordinate
 77control_forces = sm.Matrix([F, 0])
 78
 79
 80# ============================================================================
 81# Convert to MAPTOR Format
 82# ============================================================================
 83
 84lagrangian_to_maptor_dynamics(LM, [x, theta], control_forces, "cartpole_dynamics.txt")
 85
 86"""
 87Output ready to be copy-pasted:
 88
 89State variables:
 90x = phase.state('x')
 91theta = phase.state('theta')
 92x_dot = phase.state('x_dot')
 93theta_dot = phase.state('theta_dot')
 94
 95Control variables:
 96F = phase.control('F')
 97
 98MAPTOR dynamics dictionary:
 99phase.dynamics(
100    {
101        x: x_dot,
102        theta: theta_dot,
103        x_dot: (
104            4 * F + 3 * g * m * ca.sin(2 * theta) / 2 - 2 * l * m * ca.sin(theta) * theta_dot**2
105        )
106        / (4 * M + 3 * m * ca.sin(theta) ** 2 + m),
107        theta_dot: 3
108        * (
109            2 * g * (M + m) * ca.sin(theta)
110            + (2 * F - l * m * ca.sin(theta) * theta_dot**2) * ca.cos(theta)
111        )
112        / (l * (4 * M + 3 * m * ca.sin(theta) ** 2 + m)),
113    }
114)
115"""

This symbolic derivation produces the nonlinear dynamics equations used in the swing-up controller implementation, ensuring mathematical correctness and providing educational insight into the underlying mechanics.

Running This Example#

cd examples/cartpole_swingup
python cartpole_swingup.py
python cartpole_swingup_animate.py

Code Implementation#

examples/cartpole_swingup/cartpole_swingup.py#
  1import casadi as ca
  2import numpy as np
  3
  4import maptor as mtor
  5
  6
  7# ============================================================================
  8# Physical Parameters
  9# ============================================================================
 10
 11# Cart-pole system parameters
 12M = 1.0  # Cart mass (kg)
 13m = 0.1  # Pole mass (kg)
 14l = 0.5  # Pole length (m)
 15g = 9.81  # Gravity (m/s²)
 16
 17
 18# ============================================================================
 19# Problem Setup
 20# ============================================================================
 21
 22problem = mtor.Problem("Cartpole Swing-Up")
 23phase = problem.set_phase(1)
 24
 25
 26# ============================================================================
 27# Variables
 28# ============================================================================
 29
 30# Time variable
 31t = phase.time(initial=0.0)
 32
 33# State variables
 34x = phase.state("x", initial=0.0, final=0.0)  # Cart position
 35theta = phase.state("theta", initial=np.pi, final=0.0)  # Pole angle (inverted)
 36x_dot = phase.state("x_dot", initial=0.0, final=0.0)  # Cart velocity
 37theta_dot = phase.state("theta_dot", initial=0.0, final=0.0)  # Pole angular velocity
 38
 39# Control variable
 40F = phase.control("F", boundary=(-10.0, 10.0))  # Applied force on cart
 41
 42
 43# ============================================================================
 44# Dynamics obtained from cartpole_dynamics.py
 45# ============================================================================
 46
 47phase.dynamics(
 48    {
 49        x: x_dot,
 50        theta: theta_dot,
 51        x_dot: (
 52            4 * F + 3 * g * m * ca.sin(2 * theta) / 2 - 2 * l * m * ca.sin(theta) * theta_dot**2
 53        )
 54        / (4 * M + 3 * m * ca.sin(theta) ** 2 + m),
 55        theta_dot: 3
 56        * (
 57            2 * g * (M + m) * ca.sin(theta)
 58            + (2 * F - l * m * ca.sin(theta) * theta_dot**2) * ca.cos(theta)
 59        )
 60        / (l * (4 * M + 3 * m * ca.sin(theta) ** 2 + m)),
 61    }
 62)
 63
 64
 65# ============================================================================
 66# Objective
 67# ============================================================================
 68
 69# Minimize final time
 70problem.minimize(t.final)
 71
 72
 73# ============================================================================
 74# Mesh Configuration and Initial Guess
 75# ============================================================================
 76
 77phase.mesh([6, 6], [-1.0, 0.0, 1.0])
 78
 79# Initial guess - linear interpolation from initial to final states
 80states_guess = []
 81controls_guess = []
 82
 83for N in [6, 6]:
 84    tau = np.linspace(-1, 1, N + 1)
 85    t_norm = (tau + 1) / 2
 86
 87    # Linear transition from inverted (π) to upright (0)
 88    x_vals = np.zeros(N + 1)
 89    theta_vals = np.pi * (1 - t_norm)
 90    x_dot_vals = np.zeros(N + 1)
 91    theta_dot_vals = np.zeros(N + 1)
 92
 93    states_guess.append(np.vstack([x_vals, theta_vals, x_dot_vals, theta_dot_vals]))
 94
 95    # Small control guess
 96    F_vals = np.ones(N) * 0.1
 97    controls_guess.append(np.array([F_vals]))
 98
 99phase.guess(
100    states=states_guess,
101    controls=controls_guess,
102    terminal_time=2.0,
103)
104
105
106# ============================================================================
107# Solve
108# ============================================================================
109
110solution = mtor.solve_adaptive(
111    problem,
112    error_tolerance=1e-4,
113    max_iterations=20,
114    min_polynomial_degree=3,
115    max_polynomial_degree=8,
116    nlp_options={
117        "ipopt.print_level": 0,
118        "ipopt.max_iter": 500,
119        "ipopt.tol": 1e-6,
120    },
121)
122
123
124# ============================================================================
125# Results
126# ============================================================================
127
128if solution.status["success"]:
129    print(f"Objective: {solution.status['objective']:.6f}")
130    print(f"Mission time: {solution.status['total_mission_time']:.3f} seconds")
131
132    # Final states
133    x_final = solution["x"][-1]
134    theta_final = solution["theta"][-1]
135    print(f"Final cart position: {x_final:.6f} m")
136    print(f"Final pole angle: {theta_final:.6f} rad ({np.degrees(theta_final):.2f}°)")
137
138    # Control statistics
139    force_max = max(np.abs(solution["F"]))
140    print(f"Maximum control force: {force_max:.3f} N")
141
142    solution.plot()
143
144else:
145    print(f"Failed: {solution.status['message']}")