# 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 be the length of the pendulum, its mass, the moment of inertia, and the angle it makes with the vertical. Note that represent when the pendulum his hanging straight down. For a rod of length , the moment of inertia of the pendulum is , where . To see the where the equation for the moment inertia comes from see the parallel axis theorem.

Here, we let denote the center of mass of the pendulum:

Since the position of and depend only on , we can let represent our *generalized coordinate* system. Here, we will let represent our state variable, and represent our input torque.

The kinetic energy of the system is:

And the potential energy is:

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

Next, we need to calculate and to obtain the Euler-Lagrange equation:

where 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 with our input torque . 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 we are going to use an LQR controller. We first need to linearize the system about the unstable fixed point . 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()
```