Pendulum on cart derivation

In this guide, we derive the state space representation for a pendulum on a cart using Lagrange’s equations:

$L = T - V$ $\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q_i}}\right) - \frac{\partial L}{\partial q_i} = Q_i$

Free body diagram

There are two sets of masses:

• $$M_c$$; the cart mass
• $$\dot{y} = y = 0$$; i.e. we don’t expect the cart to move in the y direction
• $$M_p$$; the pendulum mass
• $$x_p = x + L \sin{\theta}$$; the pendulum x-position
• $$\dot{x_p} = \dot{x} + L \cos{\theta}$$; the pendulum x-velocity
• $$y_p = L \cos{\theta}$$; the pendulum y-position
• $$\dot{y_p} = - L \sin{\theta}$$; the pendulum y-velocity

System Kinetic Energy, T

\begin{aligned} T &= \frac{1}{2} M_c \dot{x}^2 + \frac{1}{2} M_p (\dot{x_p}^2 + \dot{y_p}^2) \\ &= \frac{1}{2}\dot{x}^2(M_c + M_p) - M_p L \dot{x}\dot{\theta}\cos{\theta} + \frac{1}{2} L^2 \dot{\theta}^2 M_p \end{aligned}

System Potential Energy, V

\begin{aligned} V &= g M_p y_p \\ &= g M_p L \cos{\theta} \end{aligned}

Lagrangian, L = T - V

$L = \frac{1}{2}\dot{x}^2(M_c + M_p) - M_p L \dot{x}\dot{\theta}\cos{\theta} + \frac{1}{2} L^2 \dot{\theta}^2 M_p - g M_p L \cos{\theta}$

Forces

In the $$x$$ coordinate:

$$F_x = F(t) - d_c \dot{x}$$

• Where: $$d_c \dot{x}$$ is the cart dampening

In the $$\theta$$ coordinate:

$$F_{\theta} = - d_p \dot{\theta}$$

• Where: $$d_c \dot{x}$$ is the pendulum dampening

Equations of motion

We now want to solve:

$\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q_i}}\right) - \frac{\partial L}{\partial q_i} = Q_i$

Where:

• $$q_i$$ represents a coordinate in a system
• i.e. in our case, we have $$(q_1, q_2) = (x, \theta)$$
• $$Q_i$$ are the forces acting on the $$i^{th}$$ coordinate
• i.e. in our case, we have $$(Q_1, Q_2) = (F_x, F_{\theta})$$

Equations of motion w.r.t x

$L = \frac{1}{2}\dot{x}^2(M_c + M_p) - M_p L \dot{x}\dot{\theta}\cos{\theta} + \frac{1}{2} L^2 \dot{\theta}^2 M_p - g M_p L \cos{\theta}$ $\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{x}}\right) - \frac{\partial L}{\partial x} = F_x$

So,

\begin{aligned} \frac{\partial L}{\partial \dot{x}} &= (M_c + M_p)\dot{x} - M_p L \dot{\theta} \cos{\theta} \\ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{x}}\right) &= (M_c + M_p)\ddot{x} - M_p L \ddot{\theta} \cos{\theta} + M_p L \dot{\theta}^2 \sin{\theta} \\ \frac{\partial L}{\partial x} &= 0 \end{aligned}

Therefore:

$(M_c + M_p)\ddot{x} - M_p L \ddot{\theta} \cos{\theta} + M_p L \dot{\theta}^2 \sin{\theta} = F(t) - d_c \dot{x}$

Equations of motion w.r.t $$\theta$$

$L = \frac{1}{2}\dot{x}^2(M_c + M_p) - M_p L \dot{x}\dot{\theta}\cos{\theta} + \frac{1}{2} L^2 \dot{\theta}^2 M_p - g M_p L \cos{\theta}$ $\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right) - \frac{\partial L}{\partial \theta} = F_{\theta}$

So,

\begin{aligned} \\ \frac{\partial L}{\partial \dot{\theta}} &= -M_p L \dot{x} \cos{\theta} + L^2 \dot{\theta} M_p \\ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right) &= -M_p L (\ddot{x} \cos{\theta} - \dot{x}\dot{\theta}\sin{\theta}) + L^2 \ddot{\theta} M_p \\ \frac{\partial L}{\partial \theta} &= \dot{x} \dot{\theta} M_p L \sin{\theta} + g M_p L \sin{\theta} \\ \end{aligned}

Therefore:

$L \ddot{\theta} - \ddot{x}\cos{\theta} - g\sin{\theta} = -\dot{\theta}\frac{d_p}{M_p L}$

State space representation

Our equations of motion are:

$(M_c + M_p)\ddot{x} - M_p L \ddot{\theta} \cos{\theta} + M_p L \dot{\theta}^2 \sin{\theta} = F(t) - d_c \dot{x}$ $L \ddot{\theta} - \ddot{x}\cos{\theta} - g\sin{\theta} = -\dot{\theta}\frac{d_p}{M_p L}$

After a lot of rearranging:

\begin{aligned} \\ \ddot{x} &= \frac{F(t) - \dot{x} d_c - \dot{\theta}^2 M_p L \sin{\theta} - \dot{\theta}\frac{d_p}{L}\cos{\theta} + g M_p \cos{\theta}\sin{\theta}}{M_c + M_p - M_p\cos^2{\theta}} \\ \\ \ddot{\theta} &= \frac{1}{L}\frac{(F(t) - \dot{x} d_c - \dot{\theta}^2 M_p L \sin{\theta})\cos{\theta} - \dot{\theta} d_p \frac{M_c + M_p}{M_p L} + g (M_c + M_p) \sin{\theta}}{M_c + M_p - M_p\cos^2{\theta}} \\ \end{aligned}

We can now represent this in an ode solver:

import scipy.integrate as integrate
import matplotlib.pyplot as plt
from matplotlib.gridspec import GridSpec
from matplotlib.animation import FuncAnimation
import numpy as np

class PendulumCart():
def __init__(self, L=1.0, mP=1.0, mC=5.0, dP=0.0, dC=0.000, g=9.81):
self.L = L
self.mP = mP
self.mC = mC
self.dP = dP
self.dC = dC
self.g = g

def _ode(self, X0, t,  inputT, inputF):
x = X0[0]
dx = X0[1]
theta = X0[2]
dtheta = X0[3]

F = np.interp(t, inputT, inputF)

s = np.sin(theta)
c = np.cos(theta)

num1 = F - (self.dC * dx) + \
(self.mP * self.L * s * dtheta ** 2.0)
num2 = c * self.g * s + \
-(c * self.dP * dtheta) / (self.L)
num3 = (self.mC + self.mP) * self.g * s + \
-(self.mC + self.mP) * (self.dP * dtheta) / (self.mP * self.L)
den = self.mC - (self.mP * s**2.0)

ddx = (num1 + num2) / den
ddtheta = (1/self.L) * (num1 * c + num3) / den

dX = np.zeros(np.size(X0))
dX[0] = dx
dX[1] = ddx
dX[2] = dtheta
dX[3] = ddtheta

return dX

def simulate(self, x_init, t, inputT, inputF):
return integrate.odeint(self._ode, x_init, t, (inputT, inputF))

class DrawCart:
def __init__(self, ax, cart):
with plt.style.context("dark_background"):
self.body, = ax.plot([], [])
self.arm, = ax.plot([], [])
self.pen, = ax.plot([], [])
self.ax = ax
self.cart = cart
ax.grid(True, linestyle=":", alpha=0.5)

def draw(self, pos, theta):
bh, bv = 0.5, 0.25
bodyx = np.array([-1.0, -1.0, 1.0, 1.0, -1.0])*bh + pos
bodyy = np.array([-1.0, 1.0, 1.0, -1.0, -1.0])*bv
self.body.set_data(bodyx, bodyy)

xp, yp = pos - np.sin(theta), np.cos(theta)
self.arm.set_data([pos, xp], [0, yp])

phi = np.linspace(-np.pi, np.pi, 32)
d = self.cart.mP/self.cart.mC

self.pen.set_data(xp + d*np.cos(phi), yp + d*np.sin(phi))

self.ax.set_xlim(pos - 3, pos + 3)
self.ax.set_ylim(-4, 4)
return self.ax, self.body, self.arm, self.pen

cart = PendulumCart(L=1.0, mP=2.0, mC=10.0, dP=1.0, dC=5.0)

T0 = 0.0
TN = 10.0
t = np.linspace(T0, TN, num=5000)

x_init = [0.0, 0.0, np.pi*0.1, 0.0]

inputT = [T0, 5.0, 5.001, 5.5, 5.501, TN]
inputF = [0.0, 0.0, 200.0, 200.0, 0.0, 0.0]

x = cart.simulate(x_init, t, inputT, inputF)

with plt.style.context("dark_background"):
color1 = plt.rcParams['axes.prop_cycle'].by_key()['color'][0]
color2 = plt.rcParams['axes.prop_cycle'].by_key()['color'][1]
color3 = plt.rcParams['axes.prop_cycle'].by_key()['color'][2]
color4 = plt.rcParams['axes.prop_cycle'].by_key()['color'][3]
color5 = plt.rcParams['axes.prop_cycle'].by_key()['color'][4]
color6 = plt.rcParams['axes.prop_cycle'].by_key()['color'][5]

fig = plt.figure(figsize=(10, 5))
gs = GridSpec(3, 2, figure=fig)
ax11 = ax1.twinx()
ax21 = ax2.twinx()

points = np.full(5, None)

ax1.plot(t, x[:, 0], color=color1)

ax1.set_ylabel(r'x (m)', color=color1)
ax11.plot(t, x[:, 1], color=color2)

ax11.set_ylabel(r'$\dot{x}$ ($\frac{m}{s}$)', color=color2)

ax2.plot(t, x[:, 2], color=color3)
ax2.set_ylabel(r'$\theta$ (rad)', color=color3)
ax21.plot(t, x[:, 3], color=color4)
ax21.set_ylabel(r'$\dot{\theta}$ ($\frac{rad}{s}$)', color=color4)

ax3.plot(inputT, inputF, ':', color=color5)
ax3.set_ylabel(r'F (N)', color=color5)

dc = DrawCart(ax0, cart)

points[0], = ax1.plot([], [], 'o', color=color1)
points[1], = ax11.plot([], [], 'o', color=color2)
points[2], = ax2.plot([], [], 'o', color=color3)
points[3], = ax21.plot([], [], 'o', color=color4)
points[4], = ax3.plot([], [], 'o', color=color5)

def animate(i):
time = (i % 100) * 0.1
pos = np.interp(time, t, x[:, 0])
dpos = np.interp(time, t, x[:, 1])
theta = np.interp(time, t, x[:, 2])
dtheta = np.interp(time, t, x[:, 3])
force = np.interp(time, inputT, inputF)

points[0].set_data(time, pos)
points[1].set_data(time, dpos)
points[2].set_data(time, theta)
points[3].set_data(time, dtheta)
points[4].set_data(time, force)

ax, l1, l2, l3 = dc.draw(pos, theta)

return ax, points[0], points[1], points[2], points[3], points[4],

ani = FuncAnimation(fig, animate, interval=100, save_count=100)
#ani.save('odePendulumCart.gif', writer='imagemagick')

plt.show()


Categories:

Updated: