Introduction

Several pieces of the puzzle have come together lately to really demonstrate the power of the scientific python software packages to handle complex dynamic and controls problems (i.e. IPython notebooks, matplotlib animations, python-control, and our software packages: sympy.physics.mechanics and PyDy).

This blog post by Wolfram demonstrates Mathematica's ability to symbolically derive the equations of motion for the n-link pendulum and stabilize it with an LQR controller. This blog post inspired us to replicate the example with all free and open source software.

In this example problem, we derive the equations of motion of an n-link pendulum on a laterally sliding cart and then develop a controller to stabilize it. Balancing a single inverted pendulum is a classic problem that is often a student's first experience with non-linear dynamics and control. The problem here is extended to a general n-link pendulum in which the equations of motion quickly get messy with greater than 2 links.

The diagram below shows the general description of the problem.


In [1]:
from IPython.display import SVG
SVG(filename='n-pendulum-with-cart.svg')


Out[1]:
image/svg+xml

Setup

This example depends on the following software:

  • IPython
  • NumPy
  • SciPy
  • SymPy >= 0.7.6
  • matplotlib

The easiest way to install the Python packages it is to use conda:

$ conda install ipython-notebook numpy scipy sympy matplotlib

To create animations you need a video encoder like ffmpeg installed.

Equations of Motion

We'll start by generating the equations of motion for the system with SymPy mechanics. The functionality that mechanics provides is much more in depth than Mathematica's functionality. In the Mathematica example, Lagrangian mechanics were implemented manually with Mathematica's symbolic functionality. mechanics provides an assortment of functions and classes to derive the equations of motion for arbitrarily complex (i.e. configuration constraints, nonholonomic motion constraints, etc) multibody systems in a very natural way. First we import the necessary functionality from SymPy.


In [2]:
from __future__ import division, print_function

In [3]:
import sympy as sm
import sympy.physics.mechanics as me

We can enable mathematical rendering of the resulting equations in the notebook with the following command.


In [4]:
me.init_vprinting()

Now specify the number of links, $n$. I'll start with 5 since the Wolfram folks only showed four.


In [5]:
n = 5

mechanics will need the generalized coordinates, generalized speeds, and the input force which are all time dependent variables and the bob masses, link lengths, and acceleration due to gravity which are all constants. Time, $t$, is also made available because we will need to differentiate with respect to time.


In [6]:
q = me.dynamicsymbols('q:{}'.format(n + 1))  # Generalized coordinates
u = me.dynamicsymbols('u:{}'.format(n + 1))  # Generalized speeds
f = me.dynamicsymbols('f')                   # Force applied to the cart
    
m = sm.symbols('m:{}'.format(n + 1))         # Mass of each bob
l = sm.symbols('l:{}'.format(n))             # Length of each link
g, t = sm.symbols('g t')                     # Gravity and time

Now we can create and inertial reference frame $I$ and define the point, $O$, as the origin.


In [7]:
I = me.ReferenceFrame('I')  # Inertial reference frame
O = me.Point('O')           # Origin point
O.set_vel(I, 0)             # Origin's velocity is zero

Secondly, we define the define the first point of the pendulum as a particle which has mass. This point can only move laterally and represents the motion of the "cart".


In [8]:
P0 = me.Point('P0')                 # Hinge point of top link
P0.set_pos(O, q[0] * I.x)           # Set the position of P0    
P0.set_vel(I, u[0] * I.x)           # Set the velocity of P0
Pa0 = me.Particle('Pa0', P0, m[0])  # Define a particle at P0

Now we can define the $n$ reference frames, particles, gravitational forces, and kinematical differential equations for each of the pendulum links. This is easily done with a loop.


In [9]:
frames = [I]                              # List to hold the n + 1 frames
points = [P0]                             # List to hold the n + 1 points
particles = [Pa0]                         # List to hold the n + 1 particles
forces = [(P0, f * I.x - m[0] * g * I.y)] # List to hold the n + 1 applied forces, including the input force, f
kindiffs = [q[0].diff(t) - u[0]]          # List to hold kinematic ODE's

for i in range(n):
    Bi = I.orientnew('B' + str(i), 'Axis', [q[i + 1], I.z])   # Create a new frame
    Bi.set_ang_vel(I, u[i + 1] * I.z)                         # Set angular velocity
    frames.append(Bi)                                         # Add it to the frames list

    Pi = points[-1].locatenew('P' + str(i + 1), l[i] * Bi.x)  # Create a new point
    Pi.v2pt_theory(points[-1], I, Bi)                         # Set the velocity
    points.append(Pi)                                         # Add it to the points list
    
    Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])        # Create a new particle
    particles.append(Pai)                                     # Add it to the particles list

    forces.append((Pi, -m[i + 1] * g * I.y))                  # Set the force applied at the point
        
    kindiffs.append(q[i + 1].diff(t) - u[i + 1])              # Define the kinematic ODE:  dq_i / dt - u_i = 0

With all of the necessary point velocities and particle masses defined, the KanesMethod class can be used to derive the equations of motion of the system automatically.


In [10]:
kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)  # Initialize the object
fr, frstar = kane.kanes_equations(forces, particles)         # Generate EoM's fr + frstar = 0

The equations of motion are quite long as can been seen below. This is the general nature of most non-simple mutlibody problems. That is why a SymPy is so useful; no more mistakes in algebra, differentiation, or copying hand written equations. Note that trigsimp can take quite a while to complete for extremely large expressions. Below we print $\tilde{M}$ and $\tilde{f}$ from $\tilde{M}\dot{u}=\tilde{f}$ to show the size of the expressions.


In [11]:
sm.trigsimp(kane.mass_matrix)


Out[11]:
$$\left[\begin{matrix}m_{0} + m_{1} + m_{2} + m_{3} + m_{4} + m_{5} & - l_{0} \left(m_{1} + m_{2} + m_{3} + m_{4} + m_{5}\right) \operatorname{sin}\left(q_{1}\right) & - l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) \operatorname{sin}\left(q_{2}\right) & - l_{2} \left(m_{3} + m_{4} + m_{5}\right) \operatorname{sin}\left(q_{3}\right) & - l_{3} \left(m_{4} + m_{5}\right) \operatorname{sin}\left(q_{4}\right) & - l_{4} m_{5} \operatorname{sin}\left(q_{5}\right)\\- l_{0} \left(m_{1} + m_{2} + m_{3} + m_{4} + m_{5}\right) \operatorname{sin}\left(q_{1}\right) & l_{0}^{2} m_{1} + l_{0}^{2} m_{2} + l_{0}^{2} m_{3} + l_{0}^{2} m_{4} + l_{0}^{2} m_{5} & l_{0} l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) \operatorname{cos}\left(q_{1} - q_{2}\right) & l_{0} l_{2} \left(m_{3} + m_{4} + m_{5}\right) \operatorname{cos}\left(q_{1} - q_{3}\right) & l_{0} l_{3} \left(m_{4} + m_{5}\right) \operatorname{cos}\left(q_{1} - q_{4}\right) & l_{0} l_{4} m_{5} \operatorname{cos}\left(q_{1} - q_{5}\right)\\- l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) \operatorname{sin}\left(q_{2}\right) & l_{0} l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) \operatorname{cos}\left(q_{1} - q_{2}\right) & l_{1}^{2} m_{2} + l_{1}^{2} m_{3} + l_{1}^{2} m_{4} + l_{1}^{2} m_{5} & l_{1} l_{2} \left(m_{3} + m_{4} + m_{5}\right) \operatorname{cos}\left(q_{2} - q_{3}\right) & l_{1} l_{3} \left(m_{4} + m_{5}\right) \operatorname{cos}\left(q_{2} - q_{4}\right) & l_{1} l_{4} m_{5} \operatorname{cos}\left(q_{2} - q_{5}\right)\\- l_{2} \left(m_{3} + m_{4} + m_{5}\right) \operatorname{sin}\left(q_{3}\right) & l_{0} l_{2} \left(m_{3} + m_{4} + m_{5}\right) \operatorname{cos}\left(q_{1} - q_{3}\right) & l_{1} l_{2} \left(m_{3} + m_{4} + m_{5}\right) \operatorname{cos}\left(q_{2} - q_{3}\right) & l_{2}^{2} m_{3} + l_{2}^{2} m_{4} + l_{2}^{2} m_{5} & l_{2} l_{3} \left(m_{4} + m_{5}\right) \operatorname{cos}\left(q_{3} - q_{4}\right) & l_{2} l_{4} m_{5} \operatorname{cos}\left(q_{3} - q_{5}\right)\\- l_{3} \left(m_{4} + m_{5}\right) \operatorname{sin}\left(q_{4}\right) & l_{0} l_{3} \left(m_{4} + m_{5}\right) \operatorname{cos}\left(q_{1} - q_{4}\right) & l_{1} l_{3} \left(m_{4} + m_{5}\right) \operatorname{cos}\left(q_{2} - q_{4}\right) & l_{2} l_{3} \left(m_{4} + m_{5}\right) \operatorname{cos}\left(q_{3} - q_{4}\right) & l_{3}^{2} m_{4} + l_{3}^{2} m_{5} & l_{3} l_{4} m_{5} \operatorname{cos}\left(q_{4} - q_{5}\right)\\- l_{4} m_{5} \operatorname{sin}\left(q_{5}\right) & l_{0} l_{4} m_{5} \operatorname{cos}\left(q_{1} - q_{5}\right) & l_{1} l_{4} m_{5} \operatorname{cos}\left(q_{2} - q_{5}\right) & l_{2} l_{4} m_{5} \operatorname{cos}\left(q_{3} - q_{5}\right) & l_{3} l_{4} m_{5} \operatorname{cos}\left(q_{4} - q_{5}\right) & l_{4}^{2} m_{5}\end{matrix}\right]$$

$\tilde{M}$ is a function of the constant parameters and the configuration.


In [12]:
me.find_dynamicsymbols(kane.mass_matrix)


Out[12]:
$$\left\{q_{1}, q_{2}, q_{3}, q_{4}, q_{5}\right\}$$

In [13]:
sm.trigsimp(kane.forcing)


Out[13]:
$$\left[\begin{matrix}l_{0} m_{1} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) + l_{0} m_{2} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) + l_{0} m_{3} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) + l_{0} m_{4} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) + l_{0} m_{5} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) + l_{1} m_{2} u^{2}_{2} \operatorname{cos}\left(q_{2}\right) + l_{1} m_{3} u^{2}_{2} \operatorname{cos}\left(q_{2}\right) + l_{1} m_{4} u^{2}_{2} \operatorname{cos}\left(q_{2}\right) + l_{1} m_{5} u^{2}_{2} \operatorname{cos}\left(q_{2}\right) + l_{2} m_{3} u^{2}_{3} \operatorname{cos}\left(q_{3}\right) + l_{2} m_{4} u^{2}_{3} \operatorname{cos}\left(q_{3}\right) + l_{2} m_{5} u^{2}_{3} \operatorname{cos}\left(q_{3}\right) + l_{3} m_{4} u^{2}_{4} \operatorname{cos}\left(q_{4}\right) + l_{3} m_{5} u^{2}_{4} \operatorname{cos}\left(q_{4}\right) + l_{4} m_{5} u^{2}_{5} \operatorname{cos}\left(q_{5}\right) + f\\- l_{0} \left(g m_{1} \operatorname{cos}\left(q_{1}\right) + g m_{2} \operatorname{cos}\left(q_{1}\right) + g m_{3} \operatorname{cos}\left(q_{1}\right) + g m_{4} \operatorname{cos}\left(q_{1}\right) + g m_{5} \operatorname{cos}\left(q_{1}\right) + l_{1} m_{2} u^{2}_{2} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{1} m_{3} u^{2}_{2} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{1} m_{4} u^{2}_{2} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{1} m_{5} u^{2}_{2} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{2} m_{3} u^{2}_{3} \operatorname{sin}\left(q_{1} - q_{3}\right) + l_{2} m_{4} u^{2}_{3} \operatorname{sin}\left(q_{1} - q_{3}\right) + l_{2} m_{5} u^{2}_{3} \operatorname{sin}\left(q_{1} - q_{3}\right) + l_{3} m_{4} u^{2}_{4} \operatorname{sin}\left(q_{1} - q_{4}\right) + l_{3} m_{5} u^{2}_{4} \operatorname{sin}\left(q_{1} - q_{4}\right) + l_{4} m_{5} u^{2}_{5} \operatorname{sin}\left(q_{1} - q_{5}\right)\right)\\l_{1} \left(- g m_{2} \operatorname{cos}\left(q_{2}\right) - g m_{3} \operatorname{cos}\left(q_{2}\right) - g m_{4} \operatorname{cos}\left(q_{2}\right) - g m_{5} \operatorname{cos}\left(q_{2}\right) + l_{0} m_{2} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{0} m_{3} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{0} m_{4} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{0} m_{5} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{2}\right) - l_{2} m_{3} u^{2}_{3} \operatorname{sin}\left(q_{2} - q_{3}\right) - l_{2} m_{4} u^{2}_{3} \operatorname{sin}\left(q_{2} - q_{3}\right) - l_{2} m_{5} u^{2}_{3} \operatorname{sin}\left(q_{2} - q_{3}\right) - l_{3} m_{4} u^{2}_{4} \operatorname{sin}\left(q_{2} - q_{4}\right) - l_{3} m_{5} u^{2}_{4} \operatorname{sin}\left(q_{2} - q_{4}\right) - l_{4} m_{5} u^{2}_{5} \operatorname{sin}\left(q_{2} - q_{5}\right)\right)\\l_{2} \left(- g m_{3} \operatorname{cos}\left(q_{3}\right) - g m_{4} \operatorname{cos}\left(q_{3}\right) - g m_{5} \operatorname{cos}\left(q_{3}\right) + l_{0} m_{3} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{3}\right) + l_{0} m_{4} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{3}\right) + l_{0} m_{5} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{3}\right) + l_{1} m_{3} u^{2}_{2} \operatorname{sin}\left(q_{2} - q_{3}\right) + l_{1} m_{4} u^{2}_{2} \operatorname{sin}\left(q_{2} - q_{3}\right) + l_{1} m_{5} u^{2}_{2} \operatorname{sin}\left(q_{2} - q_{3}\right) - l_{3} m_{4} u^{2}_{4} \operatorname{sin}\left(q_{3} - q_{4}\right) - l_{3} m_{5} u^{2}_{4} \operatorname{sin}\left(q_{3} - q_{4}\right) - l_{4} m_{5} u^{2}_{5} \operatorname{sin}\left(q_{3} - q_{5}\right)\right)\\l_{3} \left(- g m_{4} \operatorname{cos}\left(q_{4}\right) - g m_{5} \operatorname{cos}\left(q_{4}\right) + l_{0} m_{4} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{4}\right) + l_{0} m_{5} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{4}\right) + l_{1} m_{4} u^{2}_{2} \operatorname{sin}\left(q_{2} - q_{4}\right) + l_{1} m_{5} u^{2}_{2} \operatorname{sin}\left(q_{2} - q_{4}\right) + l_{2} m_{4} u^{2}_{3} \operatorname{sin}\left(q_{3} - q_{4}\right) + l_{2} m_{5} u^{2}_{3} \operatorname{sin}\left(q_{3} - q_{4}\right) - l_{4} m_{5} u^{2}_{5} \operatorname{sin}\left(q_{4} - q_{5}\right)\right)\\l_{4} m_{5} \left(- g \operatorname{cos}\left(q_{5}\right) + l_{0} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{5}\right) + l_{1} u^{2}_{2} \operatorname{sin}\left(q_{2} - q_{5}\right) + l_{2} u^{2}_{3} \operatorname{sin}\left(q_{3} - q_{5}\right) + l_{3} u^{2}_{4} \operatorname{sin}\left(q_{4} - q_{5}\right)\right)\end{matrix}\right]$$

$\tilde{f}$ is a function of the constant parameters, configuration, speeds, and the applied force.


In [14]:
me.find_dynamicsymbols(kane.forcing)


Out[14]:
$$\left\{f, q_{1}, q_{2}, q_{3}, q_{4}, q_{5}, u_{1}, u_{2}, u_{3}, u_{4}, u_{5}\right\}$$

Simulation

Now that the symbolic equations of motion are available we can simulate the pendulum's motion. We will need some more SymPy functionality and several NumPy functions, and most importantly the integration function from SciPy, odeint.


In [15]:
import numpy as np
from numpy.linalg import solve
from scipy.integrate import odeint

First, define some numeric values for all of the constant parameters in the problem.


In [16]:
arm_length = 1. / n                          # The maximum length of the pendulum is 1 meter
bob_mass = 0.01 / n                          # The maximum mass of the bobs is 10 grams
parameters = [g, m[0]]                       # Parameter definitions starting with gravity and the first bob
parameter_vals = [9.81, 0.01 / n]            # Numerical values for the first two
for i in range(n):                           # Then each mass and length
    parameters += [l[i], m[i + 1]]            
    parameter_vals += [arm_length, bob_mass]

Mathematica has a really nice NDSolve function for quickly integrating their symbolic differential equations. We make use of SymPy's lambdify function to do something similar, i.e. to create functions that will evaluate the "full" mass matrix, $M$, and "full" forcing vector, $f$ from $M\dot{x} = f(x, r, t)$ as a NumPy function.


In [17]:
dynamic = q + u    # Make a list of the states
dynamic.append(f)  # Add the input force

M_func = sm.lambdify(dynamic + parameters, kane.mass_matrix_full) # Create a callable function to evaluate the mass matrix 
f_func = sm.lambdify(dynamic + parameters, kane.forcing_full)     # Create a callable function to evaluate the forcing vector

To integrate the ODE's we need to define a function that returns the derivatives of the states given the current state and time.


In [18]:
def right_hand_side(x, t, args):
    """Returns the derivatives of the states.

    Parameters
    ----------
    x : ndarray, shape(2 * (n + 1))
        The current state vector.
    t : float
        The current time.
    args : ndarray
        The constants.

    Returns
    -------
    dx : ndarray, shape(2 * (n + 1))
        The derivative of the state.
    
    """
    r = 0.0                                  # The input force is always zero     
    arguments = np.hstack((x, r, args))      # States, input, and parameters
    dx = np.array(solve(M_func(*arguments),  # Solving for the derivatives
                  f_func(*arguments))).T[0]
    
    return dx

Now that we have the right hand side function, the initial conditions are set such that the pendulum is in the vertical equilibrium and a slight initial rate is set for each speed to ensure the pendulum falls. The equations can then be integrated with SciPy's odeint function given a time series.


In [19]:
x0 = np.hstack((0.0,                              # q0
                np.pi / 2 * np.ones(len(q) - 1),  # q1...qn+1
                1e-3 * np.ones(len(u))))          # u0...un+1

In [20]:
t = np.linspace(0.0, 10.0, num=500)  # Time vector

In [21]:
x = odeint(right_hand_side, x0, t, args=(parameter_vals,))  # Numerical integration

Plotting

The results of the simulation can be plotted with matplotlib. First, load the plotting functionality.


In [22]:
import matplotlib.pyplot as plt

In [23]:
%matplotlib inline

In [24]:
from IPython.core.pylabtools import figsize
figsize(8.0, 6.0)

The coordinate trajectories are plotted below.


In [25]:
lines = plt.plot(t, x[:, :x.shape[1] // 2])
lab = plt.xlabel('Time [sec]')
leg = plt.legend(dynamic[:x.shape[1] // 2])


And the generalized speed trajectories.


In [26]:
lines = plt.plot(t, x[:, x.shape[1] // 2:])
lab = plt.xlabel('Time [sec]')
leg = plt.legend(dynamic[x.shape[1] // 2:])


Animation

matplotlib now includes very nice animation functions for animating matplotlib plots. First we import the necessary functions for creating the animation.


In [27]:
from matplotlib import animation
from matplotlib.patches import Rectangle

The following function was modeled from Jake Vanderplas's post on matplotlib animations. The default animation writer is used (typically ffmpeg), you can change it by adding writer argument to anim.save call.


In [28]:
def animate_pendulum(t, states, length, filename=None):
    """Animates the n-pendulum and optionally saves it to file.

    Parameters
    ----------
    t : ndarray, shape(m)
        Time array.
    states: ndarray, shape(m,p)
        State time history.
    length: float
        The length of the pendulum links.
    filename: string or None, optional
        If true a movie file will be saved of the animation. This may take some time.

    Returns
    -------
    fig : matplotlib.Figure
        The figure.
    anim : matplotlib.FuncAnimation
        The animation.

    """
    # the number of pendulum bobs
    numpoints = states.shape[1] // 2

    # first set up the figure, the axis, and the plot elements we want to animate
    fig = plt.figure()
    
    # some dimesions
    cart_width = 0.4
    cart_height = 0.2
    
    # set the limits based on the motion
    xmin = np.around(states[:, 0].min() - cart_width / 2.0, 1)
    xmax = np.around(states[:, 0].max() + cart_width / 2.0, 1)
    
    # create the axes
    ax = plt.axes(xlim=(xmin, xmax), ylim=(-1.1, 1.1), aspect='equal')
    
    # display the current time
    time_text = ax.text(0.04, 0.9, '', transform=ax.transAxes)
    
    # create a rectangular cart
    rect = Rectangle([states[0, 0] - cart_width / 2.0, -cart_height / 2],
                     cart_width, cart_height, fill=True, color='red',
                     ec='black')
    ax.add_patch(rect)
    
    # blank line for the pendulum
    line, = ax.plot([], [], lw=2, marker='o', markersize=6)

    # initialization function: plot the background of each frame
    def init():
        time_text.set_text('')
        rect.set_xy((0.0, 0.0))
        line.set_data([], [])
        return time_text, rect, line,

    # animation function: update the objects
    def animate(i):
        time_text.set_text('time = {:2.2f}'.format(t[i]))
        rect.set_xy((states[i, 0] - cart_width / 2.0, -cart_height / 2))
        x = np.hstack((states[i, 0], np.zeros((numpoints - 1))))
        y = np.zeros((numpoints))
        for j in np.arange(1, numpoints):
            x[j] = x[j - 1] + length * np.cos(states[i, j])
            y[j] = y[j - 1] + length * np.sin(states[i, j])
        line.set_data(x, y)
        return time_text, rect, line,

    # call the animator function
    anim = animation.FuncAnimation(fig, animate, frames=len(t), init_func=init,
            interval=t[-1] / len(t) * 1000, blit=True, repeat=False)
    
    # save the animation if a filename is given
    if filename is not None:
        anim.save(filename, fps=30, codec='libx264')

Now we can create the animation of the pendulum. This animation will show the open loop dynamics.


In [29]:
animate_pendulum(t, x, arm_length, filename="open-loop.mp4")



In [30]:
from IPython.display import HTML
html = \
"""
<video width="640" height="480" controls>
  <source src="open-loop.mp4" type="video/mp4">
Your browser does not support the video tag, check out the YouTube version instead: http://youtu.be/Nj3_npq7MZI.
</video>
"""
HTML(html)


Out[30]:

Controller Design

The n-link pendulum can be balanced such that all of the links are inverted above the cart by applying the correct lateral force to the cart. We can design a full state feedback controller based from a linear model of the pendulum about its upright equilibrium point. We'll start by specifying the equilibrium point and parameters in dictionaries. We make sure to use SymPy types in the equilibrium point to ensure proper cancelations in the linearization.


In [31]:
equilibrium_point = [sm.S(0)] + [sm.pi / 2] * (len(q) - 1) + [sm.S(0)] * len(u)                           
equilibrium_dict = dict(zip(q + u, equilibrium_point))
equilibrium_dict


Out[31]:
$$\left \{ q_{0} : 0, \quad q_{1} : \frac{\pi}{2}, \quad q_{2} : \frac{\pi}{2}, \quad q_{3} : \frac{\pi}{2}, \quad q_{4} : \frac{\pi}{2}, \quad q_{5} : \frac{\pi}{2}, \quad u_{0} : 0, \quad u_{1} : 0, \quad u_{2} : 0, \quad u_{3} : 0, \quad u_{4} : 0, \quad u_{5} : 0\right \}$$

The KanesMethod class has method that linearizes the forcing vector about generic state and input perturbation vectors. The equilibrium point and numerical constants can then be substituted in to give the linear system in this form: $M\dot{x}=F_Ax+F_Br$. The state and input matrices, $A$ and $B$, can then be computed by left side multiplication by the inverse of the mass matrix: $A=M^{-1}F_A$ and $B=M^{-1}F_B$.


In [32]:
M, F_A, F_B, r = kane.linearize(new_method=True, op_point=equilibrium_dict)

In [33]:
sm.simplify(M)


Out[33]:
$$\left[\begin{array}{cccccccccccc}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - m_{0} - m_{1} - m_{2} - m_{3} - m_{4} - m_{5} & l_{0} \left(m_{1} + m_{2} + m_{3} + m_{4} + m_{5}\right) & l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) & l_{2} \left(m_{3} + m_{4} + m_{5}\right) & l_{3} \left(m_{4} + m_{5}\right) & l_{4} m_{5}\\0 & 0 & 0 & 0 & 0 & 0 & l_{0} \left(m_{1} + m_{2} + m_{3} + m_{4} + m_{5}\right) & - l_{0}^{2} \left(m_{1} + m_{2} + m_{3} + m_{4} + m_{5}\right) & - l_{0} l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) & - l_{0} l_{2} \left(m_{3} + m_{4} + m_{5}\right) & - l_{0} l_{3} \left(m_{4} + m_{5}\right) & - l_{0} l_{4} m_{5}\\0 & 0 & 0 & 0 & 0 & 0 & l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) & - l_{0} l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) & - l_{1}^{2} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) & - l_{1} l_{2} \left(m_{3} + m_{4} + m_{5}\right) & - l_{1} l_{3} \left(m_{4} + m_{5}\right) & - l_{1} l_{4} m_{5}\\0 & 0 & 0 & 0 & 0 & 0 & l_{2} \left(m_{3} + m_{4} + m_{5}\right) & - l_{0} l_{2} \left(m_{3} + m_{4} + m_{5}\right) & - l_{1} l_{2} \left(m_{3} + m_{4} + m_{5}\right) & - l_{2}^{2} \left(m_{3} + m_{4} + m_{5}\right) & - l_{2} l_{3} \left(m_{4} + m_{5}\right) & - l_{2} l_{4} m_{5}\\0 & 0 & 0 & 0 & 0 & 0 & l_{3} \left(m_{4} + m_{5}\right) & - l_{0} l_{3} \left(m_{4} + m_{5}\right) & - l_{1} l_{3} \left(m_{4} + m_{5}\right) & - l_{2} l_{3} \left(m_{4} + m_{5}\right) & - l_{3}^{2} \left(m_{4} + m_{5}\right) & - l_{3} l_{4} m_{5}\\0 & 0 & 0 & 0 & 0 & 0 & l_{4} m_{5} & - l_{0} l_{4} m_{5} & - l_{1} l_{4} m_{5} & - l_{2} l_{4} m_{5} & - l_{3} l_{4} m_{5} & - l_{4}^{2} m_{5}\end{array}\right]$$

In [34]:
sm.simplify(F_A)


Out[34]:
$$\left[\begin{array}{cccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & - g l_{0} \left(m_{1} + m_{2} + m_{3} + m_{4} + m_{5}\right) & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & - g l_{1} \left(m_{2} + m_{3} + m_{4} + m_{5}\right) & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & - g l_{2} \left(m_{3} + m_{4} + m_{5}\right) & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & - g l_{3} \left(m_{4} + m_{5}\right) & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & - g l_{4} m_{5} & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$

In [35]:
sm.simplify(F_B)


Out[35]:
$$\left[\begin{matrix}0\\0\\0\\0\\0\\0\\-1\\0\\0\\0\\0\\0\end{matrix}\right]$$

Now the numerical $A$ and $B$ matrices can be formed. First substitute numerical parameter values into $M$, $F_A$, and $F_B$.


In [36]:
parameter_dict = dict(zip(parameters, parameter_vals))
parameter_dict


Out[36]:
$$\left \{ g : 9.81, \quad l_{0} : 0.2, \quad l_{1} : 0.2, \quad l_{2} : 0.2, \quad l_{3} : 0.2, \quad l_{4} : 0.2, \quad m_{0} : 0.002, \quad m_{1} : 0.002, \quad m_{2} : 0.002, \quad m_{3} : 0.002, \quad m_{4} : 0.002, \quad m_{5} : 0.002\right \}$$

In [37]:
M_num = sm.matrix2numpy(M.subs(parameter_dict), dtype=float)
F_A_num = sm.matrix2numpy(F_A.subs(parameter_dict), dtype=float)
F_B_num = sm.matrix2numpy(F_B.subs(parameter_dict), dtype=float)

In [38]:
A = np.linalg.solve(M_num, F_A_num)
B = np.linalg.solve(M_num ,F_B_num)

In [39]:
print(A)


[[  0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   1.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e+00
    0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    1.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   1.00000000e+00   0.00000000e+00   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   1.00000000e+00   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   0.00000000e+00   0.00000000e+00
    0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e+00]
 [ -0.00000000e+00   4.90500000e+01  -2.03686605e-14   8.38912725e-15
   -2.06630864e-15  -9.56017853e-17  -0.00000000e+00  -0.00000000e+00
   -0.00000000e+00  -0.00000000e+00  -0.00000000e+00  -0.00000000e+00]
 [ -0.00000000e+00   4.90500000e+02  -1.96200000e+02   6.41917606e-14
   -2.48197594e-14   1.28918415e-15  -0.00000000e+00  -0.00000000e+00
   -0.00000000e+00  -0.00000000e+00  -0.00000000e+00  -0.00000000e+00]
 [ -0.00000000e+00  -2.45250000e+02   3.92400000e+02  -1.47150000e+02
    2.55272413e-14  -2.97732081e-15  -0.00000000e+00  -0.00000000e+00
   -0.00000000e+00  -0.00000000e+00  -0.00000000e+00  -0.00000000e+00]
 [ -0.00000000e+00   2.17234564e-13  -1.96200000e+02   2.94300000e+02
   -9.81000000e+01   7.68719940e-15  -0.00000000e+00  -0.00000000e+00
   -0.00000000e+00  -0.00000000e+00  -0.00000000e+00  -0.00000000e+00]
 [ -0.00000000e+00  -1.08617282e-13   5.97050187e-14  -1.47150000e+02
    1.96200000e+02  -4.90500000e+01  -0.00000000e+00  -0.00000000e+00
   -0.00000000e+00  -0.00000000e+00  -0.00000000e+00  -0.00000000e+00]
 [ -0.00000000e+00  -0.00000000e+00  -0.00000000e+00  -0.00000000e+00
   -9.81000000e+01   9.81000000e+01  -0.00000000e+00  -0.00000000e+00
   -0.00000000e+00  -0.00000000e+00  -0.00000000e+00  -0.00000000e+00]]

In [40]:
print(B)


[[  0.00000000e+00]
 [  0.00000000e+00]
 [  0.00000000e+00]
 [  0.00000000e+00]
 [  0.00000000e+00]
 [  0.00000000e+00]
 [  5.00000000e+02]
 [  2.50000000e+03]
 [ -1.15648232e-12]
 [  1.09865820e-12]
 [ -5.49329101e-13]
 [ -0.00000000e+00]]

Also convert equilibrium_point to a numeric array:


In [41]:
equilibrium_point = np.asarray([x.evalf() for x in equilibrium_point], dtype=float)

Now that we have a linear system, the SciPy package can be used to design an optimal controller for the system.


In [42]:
from numpy.linalg import matrix_rank
from scipy.linalg import solve_continuous_are

First we can check to see if the system is, in fact, controllable. The rank of the controllability matrix must be equal to the number of rows in $A$, but the matrix_rank algorithm is numerically ill conditioned and for certain values of $n$ this will fail, as seen below for $n=5$. Nevertheless, the system is controllable, no matter the number of links.


In [43]:
def controllable(a, b):
    """Returns true if the system is controllable and false if not.
    Parameters
    ----------
    a : array_like, shape(n,n)
        The state matrix.
    b : array_like, shape(n,r)
        The input matrix.
    Returns
    -------
    controllable : boolean
    """
    a = np.matrix(a)
    b = np.matrix(b)
    n = a.shape[0]
    controllability_matrix = []
    for i in range(n):
        controllability_matrix.append(a ** i * b)
    controllability_matrix = np.hstack(controllability_matrix)

    return np.linalg.matrix_rank(controllability_matrix) == n

In [44]:
controllable(A, B)


Out[44]:
False

So now we can compute the optimal gains with a linear quadratic regulator. I chose identity matrices for the weightings for simplicity.


In [45]:
Q = np.eye(A.shape[0])
R = np.eye(B.shape[1])
S = solve_continuous_are(A, B, Q, R);
K = np.dot(np.dot(np.linalg.inv(R), B.T),  S)
K


Out[45]:
array([[ -1.00000000e+00,   1.29532812e+02,  -1.37158051e+03,
          4.40234988e+03,  -5.53150156e+03,   2.39437707e+03,
         -2.38774314e+00,   1.54687844e+00,  -3.17903386e+01,
          1.74920210e+02,  -3.16052315e+02,   1.84987130e+02]])

The gains can now be used to define the required input during simulation to stabilize the system. The input $r$ is simply the gain vector multiplied by the error in the state vector from the equilibrium point, $r(t)=K(x_{eq} - x(t))$.


In [46]:
def right_hand_side(x, t, args):
    """Returns the derivatives of the states.

    Parameters
    ----------
    x : ndarray, shape(2 * (n + 1))
        The current state vector.
    t : float
        The current time.
    args : ndarray
        The constants.

    Returns
    -------
    dx : ndarray, shape(2 * (n + 1))
        The derivative of the state.
    
    """
    r = np.dot(K, equilibrium_point - x)    # The controller     
    arguments = np.hstack((x, r, args))     # States, input, and parameters
    dx = np.array(solve(M_func(*arguments), # Solving for the derivatives
                        f_func(*arguments))).T[0]
    
    return dx

Now we can simulate and animate the system to see if the controller works.


In [47]:
x0 = np.hstack((0,
                np.pi / 2 * np.ones(len(q) - 1),
                1 * np.ones(len(u))))
t = np.linspace(0.0, 10.0, num=500)
x = odeint(right_hand_side, x0, t, args=(parameter_vals,))

The plots show that we seem to have a stable system.


In [48]:
lines = plt.plot(t, x[:, :x.shape[1] // 2])
lab = plt.xlabel('Time [sec]')
leg = plt.legend(dynamic[:x.shape[1] // 2])



In [49]:
lines = plt.plot(t, x[:, x.shape[1] // 2:])
lab = plt.xlabel('Time [sec]')
leg = plt.legend(dynamic[x.shape[1] // 2:])



In [50]:
animate_pendulum(t, x, arm_length, filename="closed-loop.mp4")



In [51]:
from IPython.display import HTML
html = \
"""
<video width="640" height="480" controls>
  <source src="closed-loop.mp4" type="video/mp4">
Your browser does not support the video tag, check out the YouTube version instead: http://youtu.be/SpgBHqW9om0
</video>
"""
HTML(html)


Out[51]:

The video clearly shows that the controller can balance all $n$ of the pendulum links. The weightings in the lqr design can be tweaked to give different performance if needed.

This example shows that the free and open source scientific Python tools for dynamics are easily comparable in ability and quality to a commercial package such as Mathematica.

The IPython notebook for this example can be downloaded from https://github.com/pydy/pydy/tree/master/examples/npendulum. You can try out different $n$ values. I've gotten the equations of motion to compute for an open loop simulation of 10 links. My computer ran out of memory when I tried to compute for $n=50$. The controller weightings and initial conditions will probably have to be adjusted for better performance for $n>5$, but it should work.


In [52]:
# Install with pip install version_information
%load_ext version_information
%version_information numpy, sympy, scipy, matplotlib, control


Out[52]:
SoftwareVersion
Python3.5.3 64bit [GCC 4.4.7 20120313 (Red Hat 4.4.7-1)]
IPython4.2.0
OSLinux 4.10.0 21 generic x86_64 with debian stretch sid
numpy1.12.1
sympy1.0
scipy0.19.0
matplotlib2.0.2
controlThe 'control' distribution was not found and is required by the application
Mon May 29 13:57:02 2017 PDT