# Swing-up and Balance Control for the Simple Pendulum (under construction)

This note is under construction. The math is correct but please be aware that some sentences make no sense at all…

In this post we are going to derive an energy-based controller for the simple pendulum. First, we’ll derive the equations of motion using Lagrange’s method. Next, we’ll use a linear quadratic regulator (LQR) to balance the pendulum in the upright position. If our balancing controller works well, it should be able to recover from small perturbations. Then, we will implement controller that will cause the pendulum to swing up by pumping energy into the system. Finally, we will combine the two controllers with a switching condition so

## Deriving the Equations of Motion

Before we derive the balance and swing-up controllers, we need to first derive the equations of motion for the simple pendulum. Let $L$ be the length of the pendulum, $m$ its mass, $I$ the moment of inertia, and $\theta$ the angle it makes with the vertical. Note that $\theta = 0$ represent when the pendulum his hanging straight down. For a rod of length $L$, the moment of inertia of the pendulum is $I = I_{cm} + ml^2$, where $l = \frac{L}{2}$. To see the where the equation for the moment inertia comes from see the parallel axis theorem. Here, we let $(x_{cm}, y_{cm})$ denote the center of mass of the pendulum:

Since the position of $x_{cm}$ and $y_{cm}$ depend only on $\theta$, we can let $\theta$ represent our generalized coordinate system. Here, we will let $\textbf{q} = [\theta, \dot \theta]^T$ represent our state variable, and $u$ represent our input torque.

The kinetic energy $T$ of the system is:

And the potential energy $U$ is:

Now, we can compute the Lagrangian as the difference between the kinetic and potential energy:

Next, we need to calculate $\frac{d}{dt}\frac{\partial L}{\partial \dot{\theta}}$ and $\frac{\partial L}{\partial \theta}$ to obtain the Euler-Lagrange equation:

where $F$ represents a generalized force. Using a bit of calculus, it is easy to see:

and

Taking these equations together we get our equation for angular acceleration:

where we have replaced $F$ with our input torque $u$. Now, we can write our equations of motion:

## Balance control

Although the pendulum is a non-linear system, we can use a linear controller to balance the pendulum when it is close to the upright position. linearize the dynamics near the unstable fixed point $\theta = \pi$ we are going to use an LQR controller. We first need to linearize the system about the unstable fixed point $\theta = \pi$. More specifically we

where

def lqr(A, B, Q, R, N=None):
# Solve Algebraic Riccati Equation
P = riccati(A, B, Q, R, e=N)
R_inv = np.linalg.inv(R)
# Compute the gain matrix
if N:
K = np.dot(R_inv, np.dot(B.T, P) + N.T)
else:
K = np.dot(R_inv, np.dot(B.T, P))
return K


## Swing-up control

The total energy of the pendulum is:

# create environment
env = PendulumEnv()

# reset environment
state = env.reset()
done = False

# Get linearized dynamics
A,B = env._linearize(np.pi)

# create cost matrices for LQR
Q = np.array([[1,0], [0,1]])
R = np.array([[0.001]])

# Compute gain matrix K
K = lqr(A,B,Q,R)

# feedback gain for swing-up controller
k = 20
# Run environment
i = 0
balanced = False
while not done:
env.render()
th, thdot = state
if np.abs(th - np.pi) < .1:
balanced = True
elif np.abs(th - np.pi) > 2:
balanced = False
if balanced:
if i >= 1:
action = -np.matmul(K, state.reshape(2) - np.array([np.pi, 0]))
else:
action = -np.matmul(K, state - np.array([np.pi, 0]))
else:
# calculate energy
E = env.total_energy() - env.desired_energy()
action = -k*thdot*E

state, _, done, _ = env.step(action)
i += 1
env.close()