# Inverted Pendulum Control
## Summary
This notebook outlines state space controller design for an inverted pendulum on a cart.

This is a classic engineering school design problem. This notebook is light on theory and borrows heavily from the excellent outline by [University of Michigan Engineering Control Tutorials](http://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum&section=SystemModeling).

## Goals
The ultimate goal is to apply the control system to a simulated environment - for this I have chosen Python and specifially [pymunk](http://www.pymunk.org) as the 2D physics simulator and [pyglet](http://www.pyglet.org) as the game engine to render the results.

### Control System Requirements
The design criteria for this system with the cart receiving a 0.2 m step input are as follows:
* Settling time for x and Θ of less than 5 seconds
* Rise time for x of less than 1 second
* Overshoot of Θ less than 20 degrees (0.35 radians)

## System Description
We will use the following diagram to derive the equations of motion (image from Wikipedia):



* $\theta$ is 0 radians when vertical and is positive in the CCW direction
* $x, y$ are the position of the cart, $y$ will be constant and considered 0
* $x1, y1$ are the pendulum position
* $v1, v2$ are cart and pendulum linear velocities
* $b$ is friction, and is not shown in the image, but is a force that acts against $F$ and depends on the velocity of the cart, $v1$
* $I$ is the pendulum moment of inertia
* $M, m$ are mass, in kg, of cart and pendulum respectively
* $l$ is the distance to the pendulum center of mass
* $g$ is gravity
* $\omega$ is the angular velocity of the pendulum, and equivalent to $\frac{d}{dt}\theta$

### Equations of Motion
The equations of motion for this system are derived using the Lagrangian and Euler equations. This derivation references the following sources:
* [University of Michigan: Control Tutorials](http://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum&section=SystemModeling)
* [Wikipedia: Inverted Pendulum](https://en.wikipedia.org/wiki/Inverted_pendulum)
* [Brilliant: Lagrangian Mechanics](https://brilliant.org/wiki/lagrangian-formulation-of-mechanics/)

#### Lagrangian
The Lagrangian, L, is the difference between kinetic energy, T and potential energy V.  
$L = T - V$

In our case, we need to consider kinetic energy of the cart and pendulum (including rotational kinetic energy of the pendulum), and potential energy of the pendulum (cart is assumed to be on ground so it has no potential energy).  
$L = \frac{1}{2}Mv_{1}^2 + \frac{1}{2}mv_{2}^2 + \frac{1}{2}I\omega^2 - mgl\cos\theta$

#### Euler Equations for this system
Both x and Θ vary over time, so the Euler equations are as follows:

$\dfrac{d}{dt}\dfrac{\partial L}{\partial \dot{x}} - \dfrac{\partial L}{\partial x} = F - b\dot{x}$  

$\dfrac{d}{dt}\dfrac{\partial L}{\partial \dot{\theta}} - \dfrac{\partial L}{\partial \theta} = 0$

### derivation of equations of motion


```python
import sympy
sympy.init_printing()
```


```python
# create our functions for x and Θ against time, t
t = sympy.symbols('t')
x = sympy.Function('x')(t)
Θ = sympy.Function('Θ')(t)

# constants: pendulum length, mass, and inertia, then cart mass, gravity, and friction
l, m, I, M, g, b = sympy.symbols('l m I M g b')

# cart position is x, y, but we can ignore y since it will be constant and zero
# cart velocity is then simply the first derivate of it's x position
v1 = x.diff(t)

# pendulum position, WRT to the cart
x2 = x - l*sympy.sin(Θ)
y2 = l*sympy.cos(Θ)

# pendulum velocity is made up of x and y velocities, found from first derivates of position
v2 = sympy.sqrt(sympy.simplify(x2.diff(t)**2 + y2.diff(t)**2))

# kinetic energy of the cart, plus kinetic energy of the pendulum, subtract potential energy of pendulum and cart
L = sympy.Rational(1, 2)*(M*v1**2 + m*v2**2 + I*Θ.diff(t)**2) - m*g*l*sympy.cos(Θ)

# cleanup a bit, expand and collect on linear and angular acceleration
L = L.expand().collect((Θ.diff(t, t), x.diff(t, t))).simplify()

# print out the Lagrangian
L
```


```python
# euler equations
F = sympy.symbols('F')
euler1 = L.diff(x.diff(t)).diff(t) - L.diff(x)
euler2 = L.diff(Θ.diff(t)).diff(t) - L.diff(Θ)

# create the equations, first arg is LHS, second is RHS
eq1 = sympy.Eq(euler1.simplify().expand().collect(x.diff(t, t)), F - b*x.diff(t))
eq2 = sympy.Eq(euler2.simplify().expand().collect(Θ.diff(t, t)), 0)

# print out the 2 equations
eq1, eq2
```


```python
# linearize our system
# we want the pendulum vertical, so linearize around Θ = 0
# sin(Θ) = Θ, cos(Θ) = 1, (d/dt(Θ))**2 = 0
linearize = [(sympy.sin(Θ), Θ), (sympy.cos(Θ), 1), (Θ.diff(t)**2, 0)]
eq1_lin = eq1.subs(linearize)
eq2_lin = eq2.subs(linearize)

# print out the linearized equations
eq1_lin, eq2_lin
```


```python
# solve our linear system for linear and angular acceleration
eq_of_mot = sympy.linsolve([eq1_lin, eq2_lin], [x.diff(t, t), Θ.diff(t, t)])

# get our linear and angular velocity equations
lin_accel = eq_of_mot.args[0][0].expand().collect((Θ, Θ.diff(t), x, x.diff(t), F)).simplify()
ang_accel = eq_of_mot.args[0][1].expand().collect((Θ, Θ.diff(t), x, x.diff(t), F)).simplify()

# print the solutions
lin_accel, ang_accel
```

#### State Space Representation 
$\dot{x(t)} = Ax(t) + Bu(t)$

$y(t) = Cx(t) + Du(t)$

$\dot{x(t)} =
\begin{bmatrix}
\dot{x} \\
\ddot{x} \\
\dot{\theta} \\
\ddot{\theta} \\
\end{bmatrix}
$ &nbsp;&nbsp;&nbsp;&nbsp; $x(t) =
\begin{bmatrix}
x \\
\dot{x} \\
\theta \\
\dot{\theta} \\
\end{bmatrix}
$

$A =
\begin{bmatrix}
  0 & 1 & 0 & 0 \\
  0 & \dfrac{-b(I + l^2m)}{I(M + m) + Ml^2m} & \dfrac{gl^2m^2}{I(M + m) + Ml^2m} & 0 \\
  0 & 0 & 0 & 1 \\
  0 & \dfrac{-blm}{I(M + m) + Ml^2m} & \dfrac{lmg(M + m)}{I(M + m) + Ml^2m} & 0
\end{bmatrix}
$ &nbsp;&nbsp;&nbsp;&nbsp; $B = 
 \begin{bmatrix}
  0 \\
  \dfrac{I + l^2m}{I(M + m) + Ml^2m} \\
  0 \\
  \dfrac{lm}{I(M + m) + Ml^2m} 
 \end{bmatrix}
$ &nbsp;&nbsp;&nbsp;&nbsp; $C = 
 \begin{bmatrix}
  1 & 0 & 0 & 0 \\
  0 & 0 & 1 & 0
 \end{bmatrix}
$ &nbsp;&nbsp;&nbsp;&nbsp; $D = 
 \begin{bmatrix}
  0 \\
  0
 \end{bmatrix}
$

### controller design


```python
# start of controller design
import numpy as np
import matplotlib.pyplot as plt
import scipy.signal
import scipy.linalg
import math
```


```python
# constants for model
constants = {
    M: 0.5,   # mass of cart
    m: 0.2,   # mass of pendulum
    b: 0.1,   # friction of cart
    l: 0.6,   # length to pendulum center of mass
    I: 0.001, # inertia of pendulum
    g: 9.81,  # gravity
}
```


```python
# state space model
p = (I*(M+m)+M*l**2*m).evalf(subs=constants)  # denominator for the A and B matrices
A = np.matrix([
    [0,              1,             0, 0], 
    [0, (-b*(I+l**2*m)/p).evalf(subs=constants), ((g*l**2*m**2)/p).evalf(subs=constants), 0], 
    [0,              0,             0, 1], 
    [0,     (-(b*l*m)/p).evalf(subs=constants), (l*m*g*(M+m)/p).evalf(subs=constants), 0]
], dtype='float64')
B = np.matrix([
    [0],
    [((I+l**2*m)/p).evalf(subs=constants)],
    [0],
    [(l*m/p).evalf(subs=constants)]
], dtype='float64')
C = np.matrix([
    [1, 0, 0, 0],
    [0, 0, 1, 0]
], dtype='float64')
D = np.matrix([
    [0],
    [0]
], dtype='float64')
```


```python
# plot the poles to look at open loop system stability
poles = scipy.linalg.eigvals(A)
plt.scatter([p.real for p in poles], [p.imag for p in poles], color='red', marker='x')
plt.axvline(0, color='k', linestyle='dashed')
plt.axhline(0, color='k', linestyle='dashed')
plt.grid(True)
plt.xlabel('Real')
plt.ylabel('Imaginary')
plt.title('Poles')
plt.show()
```


```python
# system controllability - if rank[B, AB, A^2B, A^3B] == 4, it is controllable
RM = np.concatenate((B, A * B, np.linalg.matrix_power(A, 2) * B, np.linalg.matrix_power(A, 3) * B), axis=1)
if np.linalg.matrix_rank(RM) == A.shape[0]:
    print('System is controllable')
else:
    print('System is NOT controllable!')
```

    System is controllable



```python
# system observability
OM = np.concatenate((C, C * A, C * np.linalg.matrix_power(A, 2), C * np.linalg.matrix_power(A, 3)), axis=0)
if np.linalg.matrix_rank(OM) == A.shape[0]:
    print('System is observable')
else:
    print('System is NOT observable!')
```

    System is observable


### System Overview
System is controllable and observable, and because it has a pole in the RHS, it is unstable


```python
# system has a pole in the right hand plane and looks unstable, simulate the step response with 0 initial conditions
system = scipy.signal.lti(A, B, C, D)
t = np.linspace(0, 2)
u = np.ones_like(t) * 0.2
tout, yout, xout = scipy.signal.lsim(system, u, t)

fig, ax1 = plt.subplots()
ax1.plot(tout, yout[:, :1], 'b-')
ax1.set_xlabel('time (s)')
ax1.set_ylabel('Cart Position (m)', color='b')
ax1.tick_params('y', colors='b')

ax2 = ax1.twinx()
ax2.plot(tout, yout[:, 1:], 'r-')
ax2.set_ylabel('Pendulum Angle (Radians)', color='r')
ax2.tick_params('y', colors='r')

plt.grid(True)
plt.title('Open Loop Step Response')
plt.show()
```


```python
# define an LQR function, adapted from http://www.mwm.im/lqr-controllers-with-python/
def lqr(A, B, Q, R):
    X = np.matrix(scipy.linalg.solve_continuous_are(A, B, Q, R))
    K = np.matrix(scipy.linalg.inv(R) * (B.T * X))
    return K
```


```python
# find structure of Q
C.T * C
```




    matrix([[1., 0., 0., 0.],
            [0., 0., 0., 0.],
            [0., 0., 1., 0.],
            [0., 0., 0., 0.]])




```python
# element in 0,0 position used to weight cart's position, element in 2,2 position to weight pendulum angle
x = 5000
y = 100

Q = np.matrix([
    [x, 0, 0, 0],
    [0, 0, 0, 0],
    [0, 0, y, 0],
    [0, 0, 0, 0]
])
R = np.ones((1, 1));
K = lqr(A, B, Q, R)

# simulate the system with the calculated gain
Ac = A - B * K 
system = scipy.signal.lti(Ac, B, C, D)
t = np.linspace(0, 3)
u = np.ones_like(t) * 0.2
tout, yout, xout = scipy.signal.lsim(system, u, t)

fig, ax1 = plt.subplots()
ax1.plot(tout, yout[:, :1], 'b-')
ax1.set_xlabel('time (s)')
ax1.set_ylabel('Cart Position (m)', color='b')
ax1.tick_params('y', colors='b')

ax2 = ax1.twinx()
ax2.plot(tout, yout[:, 1:], 'r-')
ax2.set_ylabel('Pendulum Angle (Radians)', color='r')
ax2.tick_params('y', colors='r')

plt.grid(True)
plt.title('Step Response with LQR Control')
plt.show()
```


```python
# adapted from: http://ctms.engin.umich.edu/CTMS/index.php?aux=Extras_rscale
def rscale(A, B, C, D, K):
    s = A.shape[0]
    Z = np.append(np.zeros((1, s)), 1)
    ABcat = np.concatenate((A, B), axis=1)
    CDcat = np.concatenate((C, D), axis=1)
    ABCDinv = np.linalg.inv(np.concatenate((ABcat, CDcat), axis=0))
    N = ABCDinv.dot(Z.T)
    Nx = N.A[0][0:s]
    Nu = N.A[0][s]
    if hasattr(K, 'gain_matrix'):
        Nbar = Nu + K.gain_matrix.dot(Nx)
    else:
        Nbar = Nu + K.dot(Nx)
    return Nbar
```

### What is 'rscale'?
We want to be able to provide the cart position (x-direction) as an input - the rscale function let's us map this input to something the system understands - in this case the x-direction force applied to the cart.

If you look at the above graph, we have applied a step input of 0.2 and this has resulted in a stable system (pendulum angle is at 0) but the x-position of the cart is not at 0.2.


```python
# let's remove steady state error - step input is 0.2m so we want the cart to reach this position
# "A different C had to be used because the rscale function will not work for multiple outputs"
Cn = C[:1]
Dn = D[:1,0]
Nbar = rscale(A, B, Cn, Dn, K)

# simulate the system with Nbar - we should see the cart position reach steady state of 0.2m
Bcn = B.dot(Nbar)
system = scipy.signal.lti(Ac, Bcn, C, D)
t = np.linspace(0, 3)
u = np.ones_like(t) * 0.2
tout, yout, xout = scipy.signal.lsim(system, u, t)

fig, ax1 = plt.subplots()
ax1.plot(tout, yout[:, :1], 'b-')
ax1.set_xlabel('time (s)')
ax1.set_ylabel('Cart Position (m)', color='b')
ax1.tick_params('y', colors='b')
ax1.grid(True)

ax2 = ax1.twinx()
ax2.plot(tout, yout[:, 1:], 'r-')
ax2.set_ylabel('Pendulum Angle (Radians)', color='r')
ax2.tick_params('y', colors='r')

plt.title('Step Response with LQR Control and Nbar')
plt.show()
```

### System Design Results
We have now met our design requirements:
* Settling time for x and Θ of less than 5 seconds
* Rise time for x of less than 1 second
* Overshoot of Θ less than 20 degrees (0.35 radians)

### Digital Controller Design
Because our physics simulation is in discrete time, we need to convert our system to discrete time and perform similar steps. We will be sampling the states 60 times per second in the simulation.


```python
# let's convert the system into discrete time, so we can simulate with pymunk physics engine and pyglet game engine
# we'll sample state 60 times per second
ts = 1.0/60.0
F, G, H, J, dt = scipy.signal.cont2discrete((A, B, C, D), ts)
dsystem = (F, G, H, J, dt)
```


```python
# test the open loop system against a step input
system = scipy.signal.dlti(F, G, H, J)
tsim = 2
samples = math.ceil(tsim / ts)
t = np.linspace(0, tsim, samples)
u = np.ones_like(t) * 0.2
tout, yout, xout = scipy.signal.dlsim(system, u)

fig, ax1 = plt.subplots()
ax1.step(tout * dt, yout[:, :1], 'b-')
ax1.set_xlabel('time (s)')
ax1.set_ylabel('Cart Position (m)', color='b')
ax1.tick_params('y', colors='b')

ax2 = ax1.twinx()
ax2.step(tout * dt, yout[:, 1:], 'r-')
ax2.set_ylabel('Pendulum Angle (Radians)', color='r')
ax2.tick_params('y', colors='r')

plt.grid(True)
plt.title('Digital Open Loop Step Response')
plt.show()
```


```python
# define an discrete time LQR function - created using the following reference:
# https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator
def dlqr(A, B, Q, R):
    P = np.matrix(scipy.linalg.solve_discrete_are(A, B, Q, R))
    K = np.matrix(scipy.linalg.inv(R + B.T * P * B) * (B.T * P * A))
    return K
```


```python
# design digital LQR controller - using same gains as before
x = 5000
y = 100
Qd = np.matrix([
    [x, 0, 0, 0],
    [0, 0, 0, 0],
    [0, 0, y, 0],
    [0, 0, 0, 0]
])
R = np.ones((1, 1));
Kd = dlqr(F, G, Qd, R)

# simulate the system with the calculated gain
Fc = F - G * Kd
system = scipy.signal.dlti(Fc, G, H, J)
tsim = 3
samples = math.ceil(tsim / ts)
t = np.linspace(0, tsim, samples)
u = np.ones_like(t) * 0.2
tout, yout, xout = scipy.signal.dlsim(system, u)

fig, ax1 = plt.subplots()
ax1.step(tout * dt, yout[:, :1], 'b-')
ax1.set_xlabel('time (s)')
ax1.set_ylabel('Cart Position (m)', color='b')
ax1.tick_params('y', colors='b')

ax2 = ax1.twinx()
ax2.step(tout * dt, yout[:, 1:], 'r-')
ax2.set_ylabel('Pendulum Angle (Radians)', color='r')
ax2.tick_params('y', colors='r')

plt.grid(True)
plt.title('Step Response with Digital LQR Control')
plt.show()
```


```python
Nbar
```




    matrix([[-70.71067812]])



### Digital Nbar
We can't use the original rscale function because it is for a continuous time (CT) system, but we can use the CT Nbar value as a starting point and manually tweak it to get our steady state to the desired 0.2 m output.


```python
# let's remove steady state error - step input is 0.2 m so we want the cart to reach this position
# we can't use rscale since it was for continuous time (CT) systems, but we can use CT Nbar as a starting point 
# and tweak it
# using trial and error - started with Nbar value
Nbard = np.matrix([[-57.25]])

# simulate the system with Nbar - we should see the cart position reach steady state of 0.2 m if Nbard is set 
# appropriately
Gcn = G * Nbard
system = scipy.signal.dlti(Fc, Gcn, H, J)
tsim = 3
samples = math.ceil(tsim / ts)
t = np.linspace(0, tsim, samples)
u = np.ones_like(t) * 0.2
tout, yout, xout = scipy.signal.dlsim(system, u)

fig, ax1 = plt.subplots()
ax1.step(tout * dt, yout[:, :1], 'b-')
ax1.set_xlabel('time (s)')
ax1.set_ylabel('Cart Position (m)', color='b')
ax1.tick_params('y', colors='b')
ax1.grid(True)

ax2 = ax1.twinx()
ax2.step(tout * dt, yout[:, 1:], 'r-')
ax2.set_ylabel('Pendulum Angle (Radians)', color='r')
ax2.tick_params('y', colors='r')

plt.title('Step Response with Digital LQR Control and Nbar')
plt.show()
```


```python
# dump out our gain matrix so we can use it in our simulation
Kd
```




    matrix([[-57.38901804, -36.24133932, 118.51380879,  28.97241832]])



### Applying this to Code
We can derive the function for our input (u) using the following diagram of the system:  
  

$\vec{u} = \vec{r}N - K\vec{x}$

Where $\vec{r}$ is our desired results (just the x-position of the cart in this case), N is our digital Nbar matrix (a 1x1 matrix in this case), K is our digital gain matrix, and $\vec{x}$ is the state vector. 

There are 4 values in the gain matrix K that correspond to the 4 states we are measuring: 
* cart x-position
* cart x-velocity
* pendulum angle from vertical
* pendulum angular velocity.

The input vector $\vec{u}$ in this case only has the x-direciton force to apply to the cart.

In practice, the code will look something like this:

```python
# this code would sit in some periodic loop (at our sampling rate, which in this case is 60 times per second)

# constants - gain matrix and Nbar
K = [-57.38901804, -36.24133932, 118.51380879,  28.97241832]
Nbar = -57.25

# the following states would be sampled
# posx is cart position (x-direction)
# velx is cart velocity (x-direction)
# ang is pendulum angle
# angv is pendulum angular velocity

# calculate the gain based on the current state
gain = K[0] * posx + K[1] * velx + K[2] * ang + K[3] * angv

# calculate the required force (u)
# ref is our desired x-position of the cart - in our examples this was 0.2 m
force = ref * NBAR - gain

# we can now apply this force to the cart center of mass (in the x-direction)
```


```python
# plot results from simulation run in pyglet/pymunk
import csv

t, x, theta = [], [], []
with open('../data/invpend.csv', 'r') as f:
    for row in csv.reader(f):
        t.append(row[0])
        x.append(row[1])
        theta.append(row[2])

# ignore first row (labels) and convert data to floats
t = [float(a) for a in t[1:]]
x = [float(a) for a in x[1:]]
theta = [float(a) for a in theta[1:]]

fig, ax1 = plt.subplots()
ax1.step(t, x, 'b-')
ax1.set_xlabel('time (s)')
ax1.set_ylabel('Cart Position (m)', color='b')
ax1.tick_params('y', colors='b')
ax1.grid(True)

ax2 = ax1.twinx()
ax2.step(t, theta, 'r-')
ax2.set_ylabel('Pendulum Angle (Radians)', color='r')
ax2.tick_params('y', colors='r')

plt.title('Response from Simulation')
plt.show()
```


```python

```

695102899cc116a4560515a53d35667e53968509
ipynb
Jupyter Notebook
notebooks/inverted-pendulum.ipynb
tristeng/control
dbf99de467e92d998f4fd078057476cdf98537c7
MIT
2020-11-27T10:49:46.000Z
2021-04-04T03:41:19.000Z
notebooks/inverted-pendulum.ipynb
tristeng/control
dbf99de467e92d998f4fd078057476cdf98537c7
MIT
notebooks/inverted-pendulum.ipynb
tristeng/control
dbf99de467e92d998f4fd078057476cdf98537c7
MIT
2021-09-23T16:07:38.000Z
2022-02-08T04:23:46.000Z
Qwen/Qwen-72B
1. YES
2. YES