$\newcommand{\mb}[1]{\mathbf{ #1 }}$
$\newcommand{\bs}[1]{\boldsymbol{ #1 }}$
$\newcommand{\bb}[1]{\mathbb{ #1 }}$

$\newcommand{\R}{\bb{R}}$

$\newcommand{\ip}[2]{\left\langle #1, #2 \right\rangle}$
$\newcommand{\norm}[1]{\left\Vert #1 \right\Vert}$

$\newcommand{\der}[2]{\frac{\mathrm{d} #1 }{\mathrm{d} #2 }}$
$\newcommand{\derp}[2]{\frac{\partial #1 }{\partial #2 }}$

# Cart Pole

Consider a cart on a frictionless track. Suppose a pendulum is attached to the cart by a frictionless joint. The cart is modeled as a point mass $m_c$ and the pendulum is modeled as a massless rigid link with point mass $m_p$ a distance $l$ away from the cart.

Let $\mathcal{I} = (\mb{i}^1, \mb{i}^2, \mb{i}^3)$ denote an inertial frame. Suppose the position of the cart is resolved in the inertial frame as $\mb{r}_{co}^{\mathcal{I}} = (x, 0, 0)$. Additionally, suppose the gravitational force acting on the pendulum is resolved in the inertial frame as $\mb{f}_g^{\mathcal{I}} = (0, 0, -m_p g)$.

Let $\mathcal{B} = (\mb{b}^1, \mb{b}^2, \mb{b}^3)$ denote a body reference frame, with $\mb{b}^2 = \mb{i}^2$. The position of the pendulum mass relative to the cart is resolved in the body frame as $\mb{r}_{pc}^\mathcal{B} = (0, 0, l)$.

The kinetic energy of the system is:

\begin{equation}
    \frac{1}{2} m_c \norm{\dot{\mb{r}}_{co}^\mathcal{I}}_2^2 + \frac{1}{2} m_p \norm{\dot{\mb{r}}_{po}^\mathcal{I}}_2^2
\end{equation}

First, note that $\dot{\mb{r}}_{co}^{\mathcal{I}} = (\dot{x}, 0, 0)$.

Next, note that $\mb{r}_{po}^\mathcal{I} = \mb{r}_{pc}^\mathcal{I} + \mb{r}_{co}^\mathcal{I} = \mb{C}_{\mathcal{I}\mathcal{B}}\mb{r}_{pc}^\mathcal{B} + \mb{r}_{co}^\mathcal{I}$, where $\mb{C}_{\mathcal{I}\mathcal{B}}$ is the direction cosine matrix (DCM) satisfying:

\begin{equation}
    \mb{C}_{\mathcal{I}\mathcal{B}} = \begin{bmatrix} \ip{\mb{i}_1}{\mb{b}_1} & \ip{\mb{i}_1}{\mb{b}_2} & \ip{\mb{i}_1}{\mb{b}_3} \\ \ip{\mb{i}_2}{\mb{b}_1} & \ip{\mb{i}_2}{\mb{b}_2} & \ip{\mb{i}_2}{\mb{b}_3} \\ \ip{\mb{i}_3}{\mb{b}_1} & \ip{\mb{i}_3}{\mb{b}_2} & \ip{\mb{i}_3}{\mb{b}_3} \end{bmatrix}.
\end{equation}

We parameterize the DCM using $\theta$, measuring the clockwise angle of the pendulum from upright in radians. In this case, the DCM is:

\begin{equation}
    \mb{C}_{\mathcal{I}\mathcal{B}} = \begin{bmatrix} \cos{\theta} & 0 & \sin{\theta} \\ 0 & 1 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} \end{bmatrix},
\end{equation}

following from $\cos{\left( \frac{\pi}{2} - \theta \right)} = \sin{\theta}$. Therefore:

\begin{equation}
    \mb{r}_{po}^\mathcal{I} = \begin{bmatrix} x + l\sin{\theta} \\ 0 \\ l\cos{\theta} \end{bmatrix}
\end{equation}

We have $\dot{\mb{r}}_{po}^\mathcal{I} = \dot{\mb{C}}_{\mathcal{I}\mathcal{B}} \mb{r}_{pc}^\mathcal{B} + \dot{\mb{r}}_{co}^\mathcal{I}$, following from $\dot{\mb{r}}_{pc}^\mathcal{B} = \mb{0}_3$ since the pendulum is rigid. The derivative of the DCM is:

\begin{equation}
    \der{{\mb{C}}_{\mathcal{I}\mathcal{B}}}{\theta} = \begin{bmatrix} -\sin{\theta} & 0 & \cos{\theta} \\ 0 & 0 & 0 \\ -\cos{\theta} & 0 & -\sin{\theta} \end{bmatrix},
\end{equation}

finally yielding:

\begin{equation}
    \dot{\mb{r}}_{po}^\mathcal{I} = \dot{\theta}  \der{\mb{C}_{\mathcal{I}\mathcal{B}}}{\theta} \mb{r}^{\mathcal{B}}_{pc} + \dot{\mb{r}}_{co}^\mathcal{I} = \begin{bmatrix} l\dot{\theta}\cos{\theta} + \dot{x} \\ 0 \\ -l\dot{\theta}\sin{\theta} \end{bmatrix}
\end{equation}

Define generalized coordinates $\mb{q} = (x, \theta)$ with configuration space $\mathcal{Q} = \R \times \bb{S}^1$, where $\bb{S}^1$ denotes the $1$-sphere. The kinetic energy can then be expressed as:

\begin{align}
    T(\mb{q}, \dot{\mb{q}}) &= \frac{1}{2} m_c \begin{bmatrix} \dot{x} \\ \dot{\theta} \end{bmatrix}^\top \begin{bmatrix} 1 & 0 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} \dot{x} \\ \dot{\theta} \end{bmatrix} + \frac{1}{2} m_p \begin{bmatrix} \dot{x} \\ \dot{\theta} \end{bmatrix}^\top \begin{bmatrix} 1 & l \cos{\theta} \\ l \cos{\theta} & l^2 \end{bmatrix} \begin{bmatrix} \dot{x} \\ \dot{\theta} \end{bmatrix}\\
    &= \frac{1}{2} \dot{\mb{q}}^\top\mb{D}(\mb{q})\dot{\mb{q}},
\end{align}

where inertia matrix function $\mb{D}: \mathcal{Q} \to \bb{S}^2_{++}$ is defined as:

\begin{equation}
    \mb{D}(\mb{q}) = \begin{bmatrix} m_c + m_p & m_p l \cos{\theta} \\ m_p l \cos{\theta} & m_pl^2 \end{bmatrix}.
\end{equation}

Note that:

\begin{equation}
    \derp{\mb{D}}{x} = \mb{0}_{2 \times 2},
\end{equation}

and:

\begin{equation}
    \derp{\mb{D}}{\theta} = \begin{bmatrix} 0 & -m_p l \sin{\theta} \\ -m_p l \sin{\theta} & 0 \end{bmatrix},
\end{equation}

so we can express:

\begin{equation}
    \derp{\mb{D}}{\mb{q}} = -m_p l \sin{\theta} (\mb{e}_1 \otimes \mb{e}_2 \otimes \mb{e}_2 + \mb{e}_2 \otimes \mb{e}_1 \otimes \mb{e}_2).
\end{equation}

The potential energy of the system is $U: \mathcal{Q} \to \R$ defined as:

\begin{equation}
    U(\mb{q}) = -\ip{\mb{f}_g^\mathcal{I}}{\mb{r}^{\mathcal{I}}_{po}} = m_p g l \cos{\theta}.
\end{equation}

Define $\mb{G}: \mathcal{Q} \to \R^2$ as:
    
\begin{equation}
    \mb{G}(\mb{q}) = \left(\derp{U}{\mb{q}}\right)^\top = \begin{bmatrix} 0 \\ -m_p g l \sin{\theta} \end{bmatrix}.
\end{equation}

Assume a force $(F, 0, 0)$ (resolved in the inertial frame) can be applied to the cart. The Euler-Lagrange equation yields:

\begin{align}
    \der{}{t} \left( \derp{T}{\dot{\mb{q}}} \right)^\top - \left( \derp{T}{\mb{q}} - \derp{U}{\mb{q}} \right)^\top &= \der{}{t} \left( \mb{D}(\mb{q})\dot{\mb{q}} \right) - \frac{1}{2}\derp{\mb{D}}{\mb{q}}(\dot{\mb{q}}, \dot{\mb{q}}, \cdot) + \mb{G}(\mb{q})\\
    &= \mb{D}(\mb{q})\ddot{\mb{q}} + \derp{\mb{D}}{\mb{q}}(\cdot, \dot{\mb{q}}, \dot{\mb{q}}) - \frac{1}{2}\derp{\mb{D}}{\mb{q}}(\dot{\mb{q}}, \dot{\mb{q}}, \cdot) + \mb{G}(\mb{q})\\
    &= \mb{B} F,
\end{align}

with static actuation matrix:

\begin{equation}
    \mb{B} = \begin{bmatrix} 1 \\ 0 \end{bmatrix}.
\end{equation}

Note that:

\begin{align}
    \derp{\mb{D}}{\mb{q}}(\cdot, \dot{\mb{q}}, \dot{\mb{q}}) - \frac{1}{2}\derp{\mb{D}}{\mb{q}}(\dot{\mb{q}}, \dot{\mb{q}}, \cdot) &= -m_p l \sin{\theta} (\mb{e}_1 \dot{\theta}\dot{\theta} + \mb{e}_2\dot{x}\dot{\theta}) + \frac{1}{2} m_p l \sin{\theta} (\dot{x}\dot{\theta} \mb{e}_2 + \dot{\theta}\dot{x} \mb{e}_2)\\
    &= \begin{bmatrix} -m_p l \dot{\theta}^2 \sin{\theta} \\ 0 \end{bmatrix}\\
    &= \mb{C}(\mb{q}, \dot{\mb{q}})\dot{\mb{q}},
\end{align}

with Coriolis terms defined as:

\begin{equation}
    \mb{C}(\mb{q}, \dot{\mb{q}}) = \begin{bmatrix} 0 & -m_p l \sin{\theta} \\ 0 & 0 \end{bmatrix}.
\end{equation}

Finally, we have:

\begin{equation}
    \mb{D}(\mb{q})\ddot{\mb{q}} + \mb{C}(\mb{q}, \dot{\mb{q}})\dot{\mb{q}} + \mb{G}(\mb{q}) = \mb{B}F
\end{equation}


```python
from numpy import array, concatenate, cos, dot, reshape, sin, zeros

from core.dynamics import RoboticDynamics

class CartPole(RoboticDynamics):
    def __init__(self, m_c, m_p, l, g=9.81):
        RoboticDynamics.__init__(self, 2, 1)
        self.params = m_c, m_p, l, g
    
    def D(self, q):
        m_c, m_p, l, _ = self.params
        _, theta = q
        return array([[m_c + m_p, m_p * l * cos(theta)], [m_p * l * cos(theta), m_p * (l ** 2)]])
    
    def C(self, q, q_dot):
        _, m_p, l, _ = self.params
        _, theta = q
        _, theta_dot = q_dot
        return array([[0, -m_p * l * theta_dot * sin(theta)], [0, 0]])
    
    def U(self, q):
        _, m_p, l, g = self.params
        _, theta = q
        return m_p * g * l * cos(theta)
    
    def G(self, q):
        _, m_p, l, g = self.params
        _, theta = q
        return array([0, -m_p * g * l * sin(theta)])
    
    def B(self, q):
        return array([[1], [0]])

m_c = 0.5
m_p = 0.25
l = 0.5
cart_pole = CartPole(m_c, m_p, l)
```

We attempt to stabilize the pendulum upright, that is, drive $\theta$ to $0$. We'll use the normal form transformation:

\begin{equation}
    \bs{\Phi}(\mb{q}, \dot{\mb{q}}) = \begin{bmatrix} \bs{\eta}(\mb{q}, \dot{\mb{q}}) \\ \mb{z}(\mb{q}, \dot{\mb{q}}) \end{bmatrix} = \begin{bmatrix} \theta \\ \dot{\theta} \\ x \\ m_p l \dot{x} \cos{\theta} + m_p l^2 \dot{\theta} \end{bmatrix}.
\end{equation}


```python
from core.dynamics import ConfigurationDynamics

class CartPoleOutput(ConfigurationDynamics):
    def __init__(self, cart_pole):
        ConfigurationDynamics.__init__(self, cart_pole, 1)
        self.cart_pole = cart_pole
    
    def y(self, q):
        return q[1:]
        
    def dydq(self, q):
        return array([[0, 1]])
    
    def d2ydq2(self, q):
        return zeros((1, 2, 2))
    
output = CartPoleOutput(cart_pole)
```


```python
from numpy import identity

from core.controllers import FBLinController, LQRController

Q = 10 * identity(2)
R = identity(1)
lqr = LQRController.build(output, Q, R)
fb_lin = FBLinController(output, lqr)
```


```python
from numpy import linspace, pi

x_0 = array([0, pi / 4, 0, 0])
ts = linspace(0, 10, 1000 + 1)

xs, us = cart_pole.simulate(x_0, fb_lin, ts)
```


```python
from matplotlib.pyplot import subplots, show, tight_layout
```


```python
_, axs = subplots(2, 2, figsize=(8, 8))
ylabels = ['$x$ (m)', '$\\theta$ (rad)', '$\\dot{x}$ (m / sec)', '$\\dot{\\theta}$ (rad / sec)']

for ax, data, ylabel in zip(axs.flatten(), xs.T, ylabels):
    ax.plot(ts, data, linewidth=3)
    ax.set_ylabel(ylabel, fontsize=16)
    ax.grid()
    
for ax in axs[-1]:
    ax.set_xlabel('$t$ (sec)', fontsize=16)
    
tight_layout()
show()
```


```python
_, ax = subplots(figsize=(4, 4))

ax.plot(ts[:-1], us, linewidth=3)
ax.grid()
ax.set_xlabel('$t$ (sec)', fontsize=16)
ax.set_ylabel('$F$ (N)', fontsize=16)

show()
```

beb6ab6f74291eb4159985d1337d242f1c134130
ipynb
Jupyter Notebook
cart-pole.ipynb
ivandariojr/core
c4dec054a3e80355ed3812d48ca2bba286584a67
MIT
2021-01-26T21:00:24.000Z
2022-02-28T23:57:50.000Z
cart-pole.ipynb
ivandariojr/core
c4dec054a3e80355ed3812d48ca2bba286584a67
MIT
2020-01-28T22:49:18.000Z
2021-12-14T08:34:39.000Z
cart-pole.ipynb
ivandariojr/core
c4dec054a3e80355ed3812d48ca2bba286584a67
MIT
2019-06-07T21:31:20.000Z
2021-12-13T01:00:02.000Z
Qwen/Qwen-72B
1. YES
2. YES