Cart-Pole Balancing: Symbolic Linearization + LQR¶
This notebook demonstrates Maxima's unique strength: symbolic differentiation flowing seamlessly into numerical control design.
We derive the nonlinear equations of motion symbolically, linearize them with diff(), design an LQR controller, and verify it on the full nonlinear system with np_cvode.
In [11]:
load("numerics")$
load("numerics-sundials")$
load("numerics-optimize")$
load("ax-plots")$
1. Nonlinear Cart-Pole Dynamics¶
State: $[x, \dot{x}, \theta, \dot{\theta}]$ where $x$ is cart position and $\theta$ is pole angle from vertical (upright = 0).
Input: $F$ (horizontal force on cart).
From the Euler-Lagrange equations:
$$\ddot{x} = \frac{F + m_p \sin\theta (l\dot{\theta}^2 + g\cos\theta)}{m_c + m_p \sin^2\theta}$$
$$\ddot{\theta} = \frac{-F\cos\theta - m_p l \dot{\theta}^2 \cos\theta \sin\theta - (m_c + m_p) g \sin\theta}{l(m_c + m_p \sin^2\theta)}$$
In [12]:
/* Physical parameters */
m_c : 1.0$ /* cart mass (kg) */
m_p : 0.1$ /* pole mass (kg) */
len : 0.5$ /* half-pole length (m) */
gv : 9.81$ /* gravity (m/s^2) */
/* Symbolic state-space form: dx/dt = f(x, F) */
f1 : x_dot$
f2 : (F + m_p * sin(theta) * (len * theta_dot^2 + gv * cos(theta)))
/ (m_c + m_p * sin(theta)^2)$
f3 : theta_dot$
f4 : (-F * cos(theta) - m_p * len * theta_dot^2 * cos(theta) * sin(theta)
- (m_c + m_p) * gv * sin(theta))
/ (len * (m_c + m_p * sin(theta)^2))$
rhs : [f1, f2, f3, f4]$
state_vars : [x, x_dot, theta, theta_dot]$
print("Dynamics defined: 4 states, 1 input")$
Dynamics defined: 4 states, 1 input
2. Symbolic Linearization¶
Maxima computes the Jacobian matrices $A = \partial f/\partial x$ and $B = \partial f/\partial u$ exactly using diff(). We then evaluate at the upright equilibrium $\theta = 0$, $\dot{x} = \dot{\theta} = 0$, $F = 0$.
In [13]:
/* Symbolic Jacobian */
A_sym : genmatrix(lambda([i,j], diff(rhs[i], state_vars[j])), 4, 4)$
B_sym : transpose(matrix(makelist(diff(rhs[i], F), i, 1, 4)))$
/* Evaluate at upright equilibrium */
eq_pt : [x=0, x_dot=0, theta=0, theta_dot=0, F=0]$
A_lin : float(subst(eq_pt, A_sym))$
B_lin : float(subst(eq_pt, B_sym))$
print("A (linearized):")$
A_lin;
print("B (linearized):")$
B_lin;
A (linearized):
matrix([0.0,1.0,0.0,0.0],[0.0,0.0,0.9810000000000001,0.0],[0.0,0.0,0.0,1.0],
[0.0,0.0,-21.582000000000004,0.0])
B (linearized):
Out[13]:
3. LQR Gain Design via Riccati Iteration¶
We solve the continuous-time algebraic Riccati equation (CARE): $$A^T P + P A - P B R^{-1} B^T P + Q = 0$$
The optimal gain is $K = R^{-1} B^T P$. We use iterative refinement starting from $P = Q$.
Penalize position ($x$) and angle ($\theta$) deviations; light penalty on velocities.
In [14]:
/* Convert to ndarrays for matrix ops */
A_nd : ndarray(A_lin)$
B_nd : ndarray(B_lin)$
/* LQR weights */
Q_nd : np_diag([1.0, 0.0, 10.0, 0.0])$ /* penalize x and theta */
R_val : 0.1$
R_nd : ndarray(matrix([R_val]))$
R_inv : ndarray(matrix([1.0/R_val]))$
/* Solve CARE by iterating the Hamiltonian method:
discretize with small dt, iterate P = Q + A'^T P A' - A'^T P B' (R + B'^T P B')^{-1} B'^T P A'
where A' = I + dt*A, B' = dt*B (Euler discretization) */
dt_ric : 0.001$
A_d : np_add(np_eye(4), np_scale(dt_ric, A_nd))$
B_d : np_scale(dt_ric, B_nd)$
Q_d : np_scale(dt_ric, Q_nd)$
R_d : ndarray(matrix([R_val * dt_ric]))$
P : np_copy(Q_nd)$
for iter : 1 thru 5000 do block([BtP, S, K_tmp, AtP, P_new],
AtP : np_matmul(np_transpose(A_d), P),
BtP : np_matmul(np_transpose(B_d), P),
S : np_add(R_d, np_matmul(BtP, B_d)),
K_tmp : np_matmul(np_inv(S), np_matmul(BtP, A_d)),
P_new : np_add(Q_d, np_sub(
np_matmul(AtP, A_d),
np_matmul(np_matmul(np_matmul(np_transpose(A_d), np_matmul(P, B_d)), np_inv(S)),
np_matmul(np_transpose(B_d), np_matmul(P, A_d))))),
P : P_new
)$
/* Extract continuous-time gain: K = R^{-1} B^T P */
K_nd : np_matmul(R_inv, np_matmul(np_transpose(B_nd), P))$
K_list : np_to_list(np_flatten(K_nd))$
print("LQR gain K:", K_list)$
LQR gain K:
[3.1713128664017756,3.4310185417136987,-7.3984620618075025,
-1.2889550094582403]
4. Nonlinear Simulation¶
Apply the linear feedback $F = -K x$ to the full nonlinear cart-pole system, starting from a 15-degree tilt.
In [15]:
/* Build closed-loop dynamics by substituting F = -K*state */
K1 : K_list[1]$ K2 : K_list[2]$ K3 : K_list[3]$ K4 : K_list[4]$
F_ctrl : -(K1*x + K2*x_dot + K3*theta + K4*theta_dot)$
f1_cl : subst(F=F_ctrl, f1)$
f2_cl : subst(F=F_ctrl, f2)$
f3_cl : subst(F=F_ctrl, f3)$
f4_cl : subst(F=F_ctrl, f4)$
/* Simulate from 15-degree initial tilt */
tspan : np_linspace(0, 5, 500)$
theta0 : 15 * float(%pi) / 180$ /* 15 degrees in radians */
y0 : [0.0, 0.0, theta0, 0.0]$
result : np_cvode([f1_cl, f2_cl, f3_cl, f4_cl],
[t, x, x_dot, theta, theta_dot], y0, tspan)$
print("Simulation complete:", np_shape(result))$
Simulation complete: [500,5]
In [16]:
/* Extract state trajectories */
t_col : np_to_list(np_slice(result, all, 0))$
x_col : np_to_list(np_slice(result, all, 1))$
xd_col : np_to_list(np_slice(result, all, 2))$
th_col : np_to_list(np_slice(result, all, 3))$
thd_col : np_to_list(np_slice(result, all, 4))$
/* Convert theta to degrees for plotting */
th_deg : map(lambda([a], a * 180 / float(%pi)), th_col)$
ax_draw2d(
line_width = 2,
color = royalblue,
name = "x (m)",
lines(t_col, x_col),
color = tomato,
name = "theta (deg)",
lines(t_col, th_deg),
title = "Cart-Pole LQR: State Response",
xlabel = "Time (s)",
ylabel = "State",
grid = true
)$
In [17]:
/* Phase portrait: theta vs theta_dot */
ax_draw2d(
line_width = 2,
color = royalblue,
name = "Trajectory",
lines(th_deg, map(lambda([a], a * 180 / float(%pi)), thd_col)),
marker_size = 8,
color = forestgreen,
name = "Start",
points([th_deg[1]], [0]),
color = tomato,
name = "End",
points([last(th_deg)], [last(thd_col) * 180 / float(%pi)]),
title = "Phase Portrait (theta vs theta-dot)",
xlabel = "theta (deg)",
ylabel = "theta-dot (deg/s)",
grid = true
)$
In [18]:
/* Control effort: F = -K*state */
F_list : map(lambda([xi, xdi, thi, thdi],
-(K1*xi + K2*xdi + K3*thi + K4*thdi)),
x_col, xd_col, th_col, thd_col)$
ax_draw2d(
line_width = 2,
color = darkorchid,
name = "F (N)",
lines(t_col, F_list),
title = "Control Force",
xlabel = "Time (s)",
ylabel = "Force (N)",
grid = true
)$
5. Domain of Attraction¶
The LQR controller is designed for small perturbations. How large an initial angle can it stabilize?
In [19]:
/* Sweep initial angles from 5 to 60 degrees */
test_angles : [5, 10, 15, 20, 30, 40, 50, 60]$
stable_angles : []$
unstable_angles : []$
for ang in test_angles do block([th0, y0_test, res, final_theta],
th0 : ang * float(%pi) / 180,
y0_test : [0.0, 0.0, th0, 0.0],
res : np_cvode([f1_cl, f2_cl, f3_cl, f4_cl],
[t, x, x_dot, theta, theta_dot], y0_test, tspan),
final_theta : abs(np_ref(res, first(np_shape(res)) - 1, 3)),
if final_theta < 0.01 then
stable_angles : append(stable_angles, [ang])
else
unstable_angles : append(unstable_angles, [ang])
)$
print("Stabilized from (deg):", stable_angles)$
print("Failed from (deg):", unstable_angles)$
Stabilized from (deg): [5,10,15,20,30,40,50,60] Failed from (deg): []
In [20]:
/* Plot trajectories for several initial angles */
plot_angles : [5, 15, 30, 45]$
colors : [royalblue, forestgreen, darkorange, tomato]$
cmd : [title = "Pole Angle vs Time (Various Initial Conditions)",
xlabel = "Time (s)", ylabel = "theta (deg)", grid = true]$
for k : 1 thru length(plot_angles) do block([ang, th0, y0_k, res_k, th_k],
ang : plot_angles[k],
th0 : ang * float(%pi) / 180,
y0_k : [0.0, 0.0, th0, 0.0],
res_k : np_cvode([f1_cl, f2_cl, f3_cl, f4_cl],
[t, x, x_dot, theta, theta_dot], y0_k, tspan),
th_k : map(lambda([a], a * 180/float(%pi)),
np_to_list(np_slice(res_k, all, 3))),
cmd : append(cmd, [line_width = 2, color = colors[k],
name = sconcat(ang, " deg"),
lines(t_col, th_k)])
)$
apply(ax_draw2d, cmd)$
Key Takeaway¶
Maxima's symbolic differentiation lets us:
- Derive the equations of motion once, symbolically
- Linearize automatically with
diff()+subst()— no manual Jacobian computation - Design a controller from the linearized model
- Verify on the full nonlinear dynamics with
np_cvode
This symbolic-numeric workflow is where Maxima shines compared to purely numerical tools.