Inverted Pendulum Simulation

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

Formulate the Problem

Create Generalized Coordinates, Speeds, and Constants

In [2]:
from sympy import symbols, trigsimp
from sympy.physics.mechanics import *
In [3]:
# Initialize the pretty-printing system
init_vprinting()
In [4]:
# Number of Links
n = 3

# Generalized Coordinates and Speeds
q = dynamicsymbols('q:' + str(n + 1))
u = dynamicsymbols('u:' + str(n + 1))

# Input Force
f = dynamicsymbols('f')

# Constants:
m = symbols('m:' + str(n + 1))         # Mass of each bob
l = symbols('l:' + str(n))             # Length of each link
g, t = symbols('g t')                  # Gravity and time

Create the world coordinate frame, and origin

In [5]:
I = ReferenceFrame('I')
O = Point('O')
O.set_vel(I, 0)

Create point attached to cart

In [6]:
P0 = O.locatenew('P0', q[0] * I.x)
P0.set_vel(I, u[0] * I.x)
Pa0 = Particle('Pa0', P0, m[0])

In [7]:
frames = [I]
points = [P0]
particles = [Pa0]
forces = [(P0, f * I.x - m[0] * g * I.y)]
kindiffs = [q[0].diff() - u[0]]

for i in range(n):
    # Create a new frame oriented with current q and u
    Bi = I.orientnew('B' + str(i), 'Axis', [q[i + 1], I.z])
    Bi.set_ang_vel(I, u[i + 1] * I.z)
    frames.append(Bi)

    # Create a new point at the origin of the frame
    Pi = points[-1].locatenew('P' + str(i + 1), l[i] * Bi.x)
    Pi.v2pt_theory(points[-1], I, Bi)
    points.append(Pi)
    
    # Create a particle at that point
    Pai = Particle('Pa' + str(i + 1), Pi, m[i + 1])
    particles.append(Pai)

    # Add gravity acting at that point
    forces.append((Pi, -m[i + 1] * g * I.y))

    # Define the kinematic equations for this point
    kindiffs.append(q[i + 1].diff(t) - u[i + 1])

Solve the Dynamic Equations using Kane's Method

In [8]:
kane = KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
fr, frstar = kane.kanes_equations(forces, particles)
In [9]:
trigsimp(fr)
Out[9]:
$$\left[\begin{matrix}f\\- g l_{0} \left(m_{1} + m_{2} + m_{3}\right) \operatorname{cos}\left(q_{1}\right)\\- g l_{1} \left(m_{2} + m_{3}\right) \operatorname{cos}\left(q_{2}\right)\\- g l_{2} m_{3} \operatorname{cos}\left(q_{3}\right)\end{matrix}\right]$$
In [10]:
trigsimp(frstar)
Out[10]:
$$\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} \left(m_{1} + m_{2} + m_{3}\right) \operatorname{sin}\left(q_{1}\right) \dot{u}_{1} + 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} \left(m_{2} + m_{3}\right) \operatorname{sin}\left(q_{2}\right) \dot{u}_{2} + l_{2} m_{3} u^{2}_{3} \operatorname{cos}\left(q_{3}\right) + l_{2} m_{3} \operatorname{sin}\left(q_{3}\right) \dot{u}_{3} - \left(m_{0} + m_{1} + m_{2} + m_{3}\right) \dot{u}_{0}\\- l_{0} l_{1} m_{2} u^{2}_{2} \operatorname{sin}\left(q_{1} - q_{2}\right) - l_{0} l_{1} m_{3} u^{2}_{2} \operatorname{sin}\left(q_{1} - q_{2}\right) - l_{0} l_{1} \left(m_{2} + m_{3}\right) \operatorname{cos}\left(q_{1} - q_{2}\right) \dot{u}_{2} - l_{0} l_{2} m_{3} u^{2}_{3} \operatorname{sin}\left(q_{1} - q_{3}\right) - l_{0} l_{2} m_{3} \operatorname{cos}\left(q_{1} - q_{3}\right) \dot{u}_{3} + l_{0} \left(m_{1} + m_{2} + m_{3}\right) \operatorname{sin}\left(q_{1}\right) \dot{u}_{0} - \left(l_{0}^{2} m_{1} + l_{0}^{2} m_{2} + l_{0}^{2} m_{3}\right) \dot{u}_{1}\\l_{0} l_{1} m_{2} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{0} l_{1} m_{3} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{2}\right) - l_{0} l_{1} \left(m_{2} + m_{3}\right) \operatorname{cos}\left(q_{1} - q_{2}\right) \dot{u}_{1} - l_{1} l_{2} m_{3} u^{2}_{3} \operatorname{sin}\left(q_{2} - q_{3}\right) - l_{1} l_{2} m_{3} \operatorname{cos}\left(q_{2} - q_{3}\right) \dot{u}_{3} + l_{1} \left(m_{2} + m_{3}\right) \operatorname{sin}\left(q_{2}\right) \dot{u}_{0} - \left(l_{1}^{2} m_{2} + l_{1}^{2} m_{3}\right) \dot{u}_{2}\\l_{2} m_{3} \left(l_{0} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{3}\right) - l_{0} \operatorname{cos}\left(q_{1} - q_{3}\right) \dot{u}_{1} + l_{1} u^{2}_{2} \operatorname{sin}\left(q_{2} - q_{3}\right) - l_{1} \operatorname{cos}\left(q_{2} - q_{3}\right) \dot{u}_{2} - l_{2} \dot{u}_{3} + \operatorname{sin}\left(q_{3}\right) \dot{u}_{0}\right)\end{matrix}\right]$$

Simulation

In [11]:
from pydy.system import System
from numpy import hstack, zeros, linspace, pi, ones
from matplotlib import pyplot as plt
%matplotlib inline
/home/jimmy/Code/py27env/lib/python2.7/site-packages/pydy/codegen/code.py:18: DeprecationWarning: This module, 'pydy.codgen.code', is deprecated. The function 'generate_ode_function' can be found in the 'pydy.codegen.ode_function_generator' module. 'CythonGenerator' has been removed, use 'pydy.codegen.cython_code.CythonMatrixGenerator' instead.
  DeprecationWarning)

Define some parameter constants

In [12]:
arm_length = 1. / n
bob_mass = 0.01 / n

# Substitution dictionary, for applying the parameters
parameter_dict = {g: 9.81}
parameter_dict.update({i: arm_length for i in l})
parameter_dict.update({i: bob_mass for i in m})
In [13]:
print(parameter_dict)
{m2: 0.0033333333333333335, m3: 0.0033333333333333335, l2: 0.3333333333333333, l0: 0.3333333333333333, l1: 0.3333333333333333, g: 9.81, m1: 0.0033333333333333335, m0: 0.0033333333333333335}
In [14]:
sys = System(kane, constants=parameter_dict)
In [15]:
x0 = hstack(( 0, pi / 2 * ones(len(q) - 1), 1e-3 * ones(len(u))))
t = linspace(0, 10, 1000)

sys.initial_conditions = dict(zip(q + u, x0))
sys.times = t
y = sys.integrate()
/home/jimmy/Code/py27env/lib/python2.7/site-packages/pydy/codegen/code.py:68: DeprecationWarning: This function is deprecated and will be removed in PyDy 0.4.0. Use the the new 'generate_ode_function' in 'pydy.codegen.ode_function_generator'
  DeprecationWarning)

Plot the Results

In [16]:
plt.plot(t, y[:, :y.shape[1] / 2])
plt.xlabel('Time [sec]')
plt.legend(q)
plt.title("Generalized Coordinates");
In [17]:
plt.plot(t, y[:, y.shape[1] / 2:])
plt.xlabel('Time [sec]')
plt.legend(u)
plt.title("Generalized Speeds");

Animate the Results

In [18]:
from util import animate_pendulum
In [19]:
%%capture
animate_pendulum(t, y, arm_length, filename="open-loop.mp4")
In [20]:
%%capture
!mplayer open-loop.mp4

Controller Design

In [21]:
from control import ctrb, lqr
from numpy.linalg import matrix_rank, eigvals
from sympy import matrix2numpy

Define an Equilibrium Point

In [22]:
equilibrium_point = hstack(( 0, pi / 2 * ones(len(q) - 1), zeros(len(u)) ))

# Create a substitution dictionary to apply the point
equilibrium_dict = dict(zip(q + u, equilibrium_point))
equilibrium_dict.update({i.diff(): 0 for i in u})

Linearize about the Equilibrium Point

In [23]:
A, B, inputs = kane.linearize(op_point=(parameter_dict, equilibrium_dict), A_and_B=True, new_method=True)
A = matrix2numpy(A, float)
B = matrix2numpy(B, float)
In [24]:
eigvals(A)
Out[24]:
array([  0.        ,   0.        ,  15.11094356,   9.86296794,
         5.24797562, -15.11094356,  -9.86296794,  -5.24797562])

Check for Controllability

In [25]:
matrix_rank(ctrb(A, B)) == A.shape[0]
Out[25]:
True

Design a LQR Controller

In [26]:
K, X, E = lqr(A, B, ones(A.shape), 1)

Simulate the Feedback System

In [27]:
sys.specifieds = {f: lambda x, t: K.dot(equilibrium_point - x)}
x0 = hstack(( 0, pi / 2 * ones(len(q) - 1), 1 * ones(len(u)) ))
sys.initial_conditions = dict(zip(q + u, x0))
In [28]:
y = sys.integrate()

Plot the Results

In [29]:
plt.plot(t, y[:, :y.shape[1] / 2])
plt.xlabel('Time [sec]')
plt.legend(q)
plt.title("Generalized Coordinates");
In [30]:
plt.plot(t, y[:, y.shape[1] / 2:])
plt.xlabel('Time [sec]')
plt.legend(u)
plt.title("Generalized Speeds");

Animate the Results

In [31]:
%%capture
animate_pendulum(t, y, arm_length, filename="closed-loop.mp4")
In [32]:
%%capture
!mplayer closed-loop.mp4

SymPy Codegen

In [33]:
from sympy.printing import ccode
from sympy.utilities.codegen import codegen
from sympy.utilities.autowrap import autowrap
from sympy import sin, sqrt, lambdify
In [34]:
a, b, c = symbols('a, b, c')
expr = a + b*c + sin(a) + sqrt(b)
expr
Out[34]:
$$a + \sqrt{b} + b c + \operatorname{sin}\left(a\right)$$
In [35]:
ccode(expr)
Out[35]:
'a + sqrt(b) + b*c + sin(a)'
In [36]:
code, header = codegen(("test", expr), "c")
In [37]:
print(header[1])
/******************************************************************************
 *                      Code generated with sympy 0.7.6                       *
 *                                                                            *
 *              See http://www.sympy.org/ for more information.               *
 *                                                                            *
 *                       This file is part of 'project'                       *
 ******************************************************************************/


#ifndef PROJECT__TEST__H
#define PROJECT__TEST__H

double test(double a, double b, double c);

#endif


In [38]:
print(code[1])
/******************************************************************************
 *                      Code generated with sympy 0.7.6                       *
 *                                                                            *
 *              See http://www.sympy.org/ for more information.               *
 *                                                                            *
 *                       This file is part of 'project'                       *
 ******************************************************************************/
#include "test.h"
#include <math.h>

double test(double a, double b, double c) {

   double test_result;
   test_result = a + sqrt(b) + b*c + sin(a);
   return test_result;

}

In [39]:
f_native = autowrap(expr)
In [40]:
f_native(1, 2, 3)
Out[40]:
$$9.25568454718$$
In [41]:
f_python = lambdify((a, b, c), expr)
In [42]:
f_python(1, 2, 3)
Out[42]:
$$9.25568454718$$
In [43]:
%timeit f_native(1, 2, 3)
1000000 loops, best of 3: 471 ns per loop
In [44]:
%timeit f_python(1, 2, 3)
1000000 loops, best of 3: 775 ns per loop