layout: post title: "Inverted Pendulum Dynamics" date: 2017-04-11 12:00:00 author: Andrew header-img: img/posts/pendulum_dynamics/balance_rock.jpg header-credit: https://unsplash.com/@nbmat

tags: programming dynamics sympy python

Today, we're talking double pendulums, kinematics and more! Before you get scared off, I promise to make it accessible (why not give physics a chance).

The task at hand is to balance an inverted double pendulum, but in this post I'm going to focus solely on modeling the system. Modeling the system is important to understand what's happening, but I also plan to use this model to test out different strategies for actually controlling the balancing.

Testing different controls strategies with a physical setup is expensive, but I don't mean monetarily. Time-wise, I could be running dozens or even hundreds of trials virtually before attempting anything with the physical motor and pendulum.

So, with that said, lets get down to business. Thankfully, the double pendulum is a well studied mechanical system, and myriad academic papers are available on the subject. One great resource I found was A Mathematical Introduction to Robotic Manipulation [^mls_textbook]

One reason the double pendulum is popular is because it's a simple example of chaotic motion. Chaotic motion means that the system is highly sensitive to input conditions. Practically, this means if you were to release the pendulum from almost exactly the same spot many times in a row you would see far different outcomes each time.

This post is going to be a bit technical, but as always, I'll try my best to make it interesting for all audiences.

First things first, let's agree on what everything's going to be called:

Here is our double pendulum system. Each link has a length $$l$$, a mass $$m$$, and it's center of mass has coordinates $$x$$, $$y$$. The center of mass is a distance $$r$$ from the base of each link. Motion will only be considered in the $$x-y$$ plane.

Mechanical systems such as these can be described by the number of degrees of freedom) that it has. The degrees of freedom of a system are the independant parameters that are needed to fully describe the configuration.

If there were a physical double pendulum in front of you, image moving the links around. You would eventually determine that there are really only two things that you would need to tell someone for them to agree that you're both looking at the same configuration of the links.

In this system, those two degrees of freedom are the angles $$\theta_0$$ and $$\theta_1$$. The state of the system can be described completely with these two angles.

What that means for us is that those two angles are what we want to simulate given some initial conditions.

WARNING: MATH

This is where we take a slight leap into some mechanics that may be a bit advanced, but try to stick it out!

There are two types of energy present in our double pendulum system:

kinetic ($$K$$), which comes from motion. In this case, that motion is the swinging action of links.
potential ($$U$$), which is the stored energy of the system. In this case, the stored energy is contained in the link's height, from gravity. Gravitational potential energy is why things roll downhill (basically).

We're going to use a function called the Lagrangian to work some magic.

Using just the potential and kinetic energy of the system, and describing the coordinates of the links, the Lagrangain can yield the equations of motion for the system.

Any function which generates the correct equations of motion, in agreement with physical laws, can be taken as a Lagrangian. It is nevertheless possible to construct general expressions for large classes of applications.

System State Equations

$$ \begin{align} x_0=& r_{0} \cos{\left (\theta_0 \right )} & \\ y_0=& r_{0} \sin{\left (\theta_0 \right )} & \\ x_1=& l_{0} \cos{\left (\theta_0 \right )} + r_{1} \cos{\left (\theta_0 + \theta_1 \right )} & \\ y_1=& l_{0} \sin{\left (\theta_0 \right )} + r_{1} \sin{\left (\theta_0 + \theta_1 \right )} & \\ \end{align} $$

Kinetic Energy

$$ \begin{align} K =& \tfrac{1}{2}I_0\omega_0^2 + \tfrac{1}{2}m_0v_0^2 + \tfrac{1}{2}I_1\omega_1^2 + \tfrac{1}{2}m_1v_1^2 & \\ \omega_0 =& \dot{\theta_0} & \\ \omega_1 =& \dot{\theta_0} + \dot{\theta_1} & \\ K =& \tfrac{1}{2}I_0\dot{\theta_0}^2 + \tfrac{1}{2}m_0v_0^2 + \tfrac{1}{2}I_1(\dot{\theta_0} + \dot{\theta_1})^2 + \tfrac{1}{2}m_1v_1^2 & \\ v =& \dot{x} + \dot{y} & \\ K =& \tfrac{1}{2}I_0\dot{\theta_0}^2 + \tfrac{1}{2}m_0(\dot{x_0}^2 + \dot{y_0}^2) + \tfrac{1}{2}I_1(\dot{\theta_0} + \dot{\theta_1})^2 + \tfrac{1}{2}m_1(\dot{x_1}^2 + \dot{y_1}^2) & \\ \end{align} $$

We'll let sympy handle the last substitution to get the equation in terms of only $$\theta$$

Potential Energy

$$ \begin{align} U =& m_0gy_0 + m_1gy_1 & \\ \end{align} $$

Lagrange

$$ L = K - U $$

Now we can substitute numbers into our equation to perform the actual calculations.



In [1]:

    
from sympy import symbols, init_printing, S, Derivative, diff, simplify, solve, lambdify, cos, sin
from sympy.physics.vector import vlatex
import numpy as np
import scipy.integrate as integrate
from matplotlib import pyplot as plt
from matplotlib import animation, rc
from itertools import chain
from IPython.display import HTML, display, Math

rc('animation', html='html5')

init_printing(latex_printer=vlatex, latex_mode='equation')



In [2]:

    
def disp(expr):
    display(Math(vlatex(simplify(expr))))
    
def disp_eq(lhs,expr):
    display(Math("$${0} = {1}$$".format(lhs, vlatex(simplify(expr)))))



In [3]:

    
def generate_double_pendulum_odes():
    """
    :return:
     List of ODE describing system (Number = DOF of system)
     List of plotting position functions (Number = DOF of system)
    """
    t = symbols('t')
    g = symbols('g')
    l = symbols('l0:2')
    m = symbols('m0:2')
    r = symbols('r0:2')
    i = symbols('I0:2')
    tau = symbols('tau0:2')
    b = symbols('b0:2')

    g_val = S(9.8)
    l_val = [S(1.0), S(1.0)]
    m_val = [S(1.0), S(1.0)]
    r_val = [temp_l / 2 for temp_l in l_val]
    i_val = [(temp_m * temp_l ** 2) / 12 for temp_m, temp_l in zip(m_val, l_val)]
    tau_val = [S(0.0), S(0.0)]
    b_val = [S(0.0), S(0.0)]

    theta = [w(t) for w in symbols('theta0:2')]
    theta_dot = [Derivative(w, t) for w in theta]
    theta_ddot = [Derivative(w, t, t) for w in theta]

    x = [None] * 2
    y = [None] * 2
    x_dot = [None] * 2
    y_dot = [None] * 2

    x[0] = r[0] * cos(theta[0])
    y[0] = r[0] * sin(theta[0])
    x[1] = l[0] * cos(theta[0]) + r[1] * cos(theta[0] + theta[1])
    y[1] = l[0] * sin(theta[0]) + r[1] * sin(theta[0] + theta[1])

    x_dot[0] = diff(x[0], t)
    y_dot[0] = diff(y[0], t)
    x_dot[1] = diff(x[1], t)
    y_dot[1] = diff(y[1], t)

    kinetic = (m[0] * (x_dot[0] ** 2 + y_dot[0] ** 2)
               + m[1] * (x_dot[1] ** 2 + y_dot[1] ** 2)
               + i[0] * (theta_dot[0]) ** 2
               + i[1] * (theta_dot[0] + theta_dot[1]) ** 2) / 2

    potential = (m[0] * g * y[0]) + (m[1] * g * y[1])

    lagrange = kinetic - potential

    lagrangian = [None] * 2
    lagrangian[0] = diff(lagrange, theta_dot[0], t) - diff(lagrange, theta[0])
    lagrangian[1] = diff(lagrange, theta_dot[1], t) - diff(lagrange, theta[1])

    solution = solve(lagrangian, theta_ddot)

    values = [(g, g_val),
              (l[0], l_val[0]),
              (l[1], l_val[1]),
              (m[0], m_val[0]),
              (m[1], m_val[1]),
              (r[0], r_val[0]),
              (r[1], r_val[1]),
              (i[0], i_val[0]),
              (i[1], i_val[1]),
              (tau[0], tau_val[0]),
              (tau[1], tau_val[1]),
              (b[0], b_val[0]),
              (b[1], b_val[1])]

    temp_vars = symbols('z0:4')

    inputs = list(zip((theta_dot[0], theta[0], theta_dot[1], theta[1]), temp_vars))

    ode_equations = [None] * 2
    ode_equations[0] = lambdify(temp_vars, simplify(solution[theta_ddot[0]]).subs(values).subs(inputs))
    ode_equations[1] = lambdify(temp_vars, simplify(solution[theta_ddot[1]]).subs(values).subs(inputs))

    def double_pendulum_position(pos):
        result = []

        for _, theta0, _, theta1 in pos:
            x1_pos = float(l_val[0]) * np.cos(theta0)
            y1_pos = float(l_val[0]) * np.sin(theta0)

            x2_pos = x1_pos + float(l_val[1]) * np.cos(theta0 + theta1)
            y2_pos = y1_pos + float(l_val[1]) * np.sin(theta0 + theta1)

            result.append(((0, x1_pos, x2_pos), (0, y1_pos, y2_pos)))

        return result

    return ode_equations, double_pendulum_position



In [4]:

    
def generic_deriv_handler(this_state, _, deriv_functions):
    # x_dot, x pairs
    result = [(float(func(*this_state)), this_state[(i * 2)]) for i, func in enumerate(deriv_functions)]

    flattened = chain.from_iterable(result)
    float_flattened = list(map(float, flattened))

    return np.array(float_flattened)



In [5]:

    
def animate_system(time, time_step, initial_conditions, derivation_functions, position_function, plot_limit):

    pos = integrate.odeint(generic_deriv_handler, np.radians(initial_conditions), np.arange(0.0, time, time_step),
                           args=(derivation_functions,))

    plot_positions = position_function(pos)

    fig = plt.figure()
    ax = fig.add_subplot(111, autoscale_on=False, xlim=(-plot_limit, plot_limit), ylim=(-plot_limit, plot_limit))
    ax.grid()
    ax.set_aspect('equal', adjustable='box')

    line, = ax.plot([], [], 'k-', lw=4, solid_capstyle='round')
    time_template = 'time = {:0.2f}s'
    time_text = ax.text(0.05, 0.9, '', transform=ax.transAxes)

    def init():
        line.set_data([], [])
        time_text.set_text('')
        return line, time_text

    def animate(i):
        # TODO position func should be generator
        # TODO dependance on DOF
        thisx, thisy = plot_positions[i]

        line.set_data(thisx, thisy)
        time_text.set_text(time_template.format(time_step))
        return line, time_text

    return animation.FuncAnimation(fig, animate, frames=len(pos), interval=25, blit=True, init_func=init)



In [6]:

    
ani = animate_system(5, 0.05, [0, 90, 0, 90], *generate_double_pendulum_odes(), 2)



In [7]:

    
HTML(ani.to_html5_video())









    Out[7]:



In [10]:

    
def generate_triple_pendulum_odes():
    """
    :return:
     List of ODE describing system (Number = DOF of system)
     List of plotting position functions (Number = DOF of system)
    """
    t = symbols('t')
    g = symbols('g')
    l = symbols('l0:3')
    m = symbols('m0:3')
    r = symbols('r0:3')
    i = symbols('I0:3')
    tau = symbols('tau0:3')
    b = symbols('b0:3')

    g_val = S(9.8)
    l_val = [S(1.0)] * 3
    m_val = [S(1.0)] * 3
    r_val = [temp_l / 2 for temp_l in l_val]
    i_val = [(temp_m * temp_l ** 2) / 12 for temp_m, temp_l in zip(m_val, l_val)]
    tau_val = [S(0.0)] * 3
    b_val = [S(0.0)] * 3

    theta = [w(t) for w in symbols('theta0:3')]
    theta_dot = [Derivative(w, t) for w in theta]
    theta_ddot = [Derivative(w, t, t) for w in theta]

    x = [None] * 3
    y = [None] * 3
    x_dot = [None] * 3
    y_dot = [None] * 3

    x[0] = r[0] * cos(theta[0])
    y[0] = r[0] * sin(theta[0])
    x[1] = l[0] * cos(theta[0]) + r[1] * cos(theta[0] + theta[1])
    y[1] = l[0] * sin(theta[0]) + r[1] * sin(theta[0] + theta[1])
    x[2] = l[0] * cos(theta[0]) + l[1] * cos(theta[0] + theta[1]) + r[2] * cos(theta[0] + theta[1] + theta[2])
    y[2] = l[0] * sin(theta[0]) + l[1] * sin(theta[0] + theta[1]) + r[2] * sin(theta[0] + theta[1] + theta[2])
    
    x_dot[0] = diff(x[0], t)
    y_dot[0] = diff(y[0], t)
    x_dot[1] = diff(x[1], t)
    y_dot[1] = diff(y[1], t)
    x_dot[2] = diff(x[2], t)
    y_dot[2] = diff(y[2], t)

    kinetic = (m[0] * (x_dot[0] ** 2 + y_dot[0] ** 2)
               + m[1] * (x_dot[1] ** 2 + y_dot[1] ** 2)
               + m[2] * (x_dot[2] ** 2 + y_dot[2] ** 2)
               + i[0] * (theta_dot[0]) ** 2
               + i[1] * (theta_dot[0] + theta_dot[1]) ** 2
               + i[2] * (theta_dot[0] + theta_dot[1] + theta_dot[2]) ** 2) / 2

    potential = (m[0] * g * y[0]) + (m[1] * g * y[1]) + (m[2] * g * y[2])

    lagrange = kinetic - potential

    lagrangian = [diff(lagrange, th_d, t) - diff(lagrange, th) for th_d,th in zip(theta_dot,theta)]

    solution = solve(lagrangian, theta_ddot)

    values = [(g, g_val),
              (l[0], l_val[0]),
              (l[1], l_val[1]),
              (l[2], l_val[2]),
              (m[0], m_val[0]),
              (m[1], m_val[1]),
              (m[2], m_val[2]),
              (r[0], r_val[0]),
              (r[1], r_val[1]),
              (r[2], r_val[2]),
              (i[0], i_val[0]),
              (i[1], i_val[1]),
              (i[2], i_val[2]),
              (tau[0], tau_val[0]),
              (tau[1], tau_val[1]),
              (tau[2], tau_val[2]),
              (b[0], b_val[0]),
              (b[1], b_val[1]),
              (b[2], b_val[2])]

    temp_vars = symbols('z0:6')

    inputs = list(zip((theta_dot[0], theta[0], theta_dot[1], theta[1], theta_dot[2], theta[2]), temp_vars))

    ode_equations = [lambdify(temp_vars, simplify(solution[th_ddot]).subs(values).subs(inputs)) for th_ddot in theta_ddot]
    
    def triple_pendulum_position(pos):
        result = []

        for _, theta0, _, theta1, _, theta2 in pos:
            x1_pos = float(l_val[0]) * np.cos(theta0)
            y1_pos = float(l_val[0]) * np.sin(theta0)

            x2_pos = x1_pos + float(l_val[1]) * np.cos(theta0 + theta1)
            y2_pos = y1_pos + float(l_val[1]) * np.sin(theta0 + theta1)

            x3_pos = x2_pos + float(l_val[2]) * np.cos(theta0 + theta1 + theta2)
            y3_pos = y2_pos + float(l_val[2]) * np.sin(theta0 + theta1 + theta2)
            
            result.append(((0, x1_pos, x2_pos, x3_pos), (0, y1_pos, y2_pos, y3_pos)))

        return result

    return ode_equations, triple_pendulum_position



In [12]:

    
ani = animate_system(5, 0.05, [0, 0, 0, 0, 0, 0], *generate_triple_pendulum_odes(), 3)



In [13]:

    
HTML(ani.to_html5_video())









    Out[13]:



In [ ]:

Author. Title. Title of container (self contained if book), Other contributors (translators or editors), Version (edition), Number (vol. and/or no.), Publisher, Publication Date, Location (pages, paragraphs URL or DOI). 2nd container’s title, Other contributors, Version, Number, Publisher, Publication date, Location, Date of Access (if applicable).



In [ ]: