Hoani

Engineering and creative things.

hoani.net is a github pages website

Please consider ☆ing the hoani.github.io repository if you have found this website helpful.

Pendulum on cart derivation

5 minute read

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)
    ax0 = fig.add_subplot(gs[:, 0])
    ax1 = fig.add_subplot(gs[0, 1])
    ax11 = ax1.twinx()
    ax2 = fig.add_subplot(gs[1, 1])
    ax21 = ax2.twinx()
    ax3 = fig.add_subplot(gs[2, 1])

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()