Setup



In [1]:

    
import sympy.physics.mechanics as me
import sympy
import control
import matplotlib.animation
import matplotlib.pyplot as plt
import scipy.integrate
import itertools
from mpl_toolkits.mplot3d.axes3d import Axes3D

# video animation
from tempfile import NamedTemporaryFile
from IPython.display import HTML

%matplotlib inline
import numpy as np
sympy.init_printing()



In [2]:

    
#plt.rcParams.keys()



In [3]:

    
plt.rcParams['lines.linewidth'] = 2
plt.rcParams['font.size'] = 12
plt.rcParams['animation.writer'] = 'avconv'
plt.rcParams['animation.codec'] = 'libx264'



In [4]:

    
def anim_to_html(anim):
    plt.close(anim._fig)
    if not hasattr(anim, '_encoded_video'):
        with NamedTemporaryFile(suffix='.mp4') as f:
            anim.save(f.name, fps=20, extra_args=['-vcodec', 'libx264'])
            video = open(f.name, "rb").read()
        anim._encoded_video = video.encode("base64")
    
    return """<video controls>
    <source src="data:video/x-m4v;base64,{0}" type="video/mp4">
     Your browser does not support the video tag.
    </video>""".format(anim._encoded_video)
matplotlib.animation.Animation._repr_html_ = anim_to_html

Definition of the System

First we will define the variables and parameters for the system.



In [5]:

    
theta, t, m, g, l = sympy.symbols('theta, t, m, g, l')
values = {'g': 9.8, 'l': 0.1, 'm': 1}

Now we will define the inertial reference frame (i), the body reference frame (b).



In [6]:

    
frame_i = me.ReferenceFrame('i') # the inertial frame
frame_b = frame_i.orientnew('b', 'Axis', (theta(t), frame_i.z))  # the body frame

Next we define the base and end of the pendulum in terms of the given coordinate frames.



In [7]:

    
point_o = me.Point('o') # the base of the pendulum
point_o.set_vel(frame_i, 0) # the base of the pendulum is fixed in the inertial frame
point_p = point_o.locatenew('p', l*frame_b.y) # the end of the pendulum
point_p.set_vel(frame_b, 0) # point p is fixed in the body frame
point_p.v1pt_theory(point_o, frame_i, frame_b)
particle_a = me.Particle('a', point_p, m) # define a point mass particle at the end of the pendulum

For this simple problem we assume that the only force acting on the pendulum is the weight due to gravity.



In [8]:

    
weight = -m*g*frame_i.y
weight









    Out[8]:





$$-  g m\mathbf{\hat{i}_y}$$

We can then calculate the moment acting on the pendulum.



In [9]:

    
u = sympy.Symbol('u') # input
d = sympy.Symbol('d') # disturbance
M_o = point_p.pos_from(point_o).cross(weight).dot(frame_b.z) + u(t) + d(t)
sympy.Eq(sympy.Symbol('M_o'), M_o)









    Out[9]:





$$M_{o} = g l m \sin{\left (\theta{\left (t \right )} \right )} + d{\left (t \right )} + u{\left (t \right )}$$

Deriving Equations of Motion

Direct Application of Euler's 2nd Law of Motion



In [10]:

    
H__i_o = particle_a.angular_momentum(point_o, frame_i).dot(frame_i.z)
sympy.Eq(sympy.Symbol('H__i_o'), H__i_o)









    Out[10]:





$$H^{i}_{o} = l^{2} m \frac{d}{d t} \theta{\left (t \right )}$$

We can then use Euler's 2nd law of rotational motion.



In [11]:

    
eom = M_o - H__i_o.diff(t)
sympy.Eq(eom, 0)









    Out[11]:





$$g l m \sin{\left (\theta{\left (t \right )} \right )} - l^{2} m \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )} + d{\left (t \right )} + u{\left (t \right )} = 0$$

This can be simplified by solving for the second derivative of $\theta$.



In [12]:

    
eom_theta_dd = sympy.solve(eom, theta(t).diff(t,2))[0]
sympy.Eq(theta(t).diff(t,2), eom_theta_dd)









    Out[12]:





$$\frac{d^{2}}{d t^{2}}  \theta{\left (t \right )} = \frac{1}{l^{2} m} \left(g l m \sin{\left (\theta{\left (t \right )} \right )} + d{\left (t \right )} + u{\left (t \right )}\right)$$

At equilibrium, if we set the derivatives of theta to zero, we get the equilibrium input.



In [13]:

    
eom_equil = eom.subs({theta(t).diff(t,2):0})
sympy.Eq(eom_equil, 0)









    Out[13]:





$$g l m \sin{\left (\theta{\left (t \right )} \right )} + d{\left (t \right )} + u{\left (t \right )} = 0$$



In [14]:

    
u_eq = sympy.solve(eom.subs({theta(t).diff(t,2):0}), u(t), theta(t))[0][u(t)]
u_eq









    Out[14]:





$$- g l m \sin{\left (\theta{\left (t \right )} \right )} - d{\left (t \right )}$$



In [15]:

    
x_vect = sympy.Matrix([theta(t), theta(t).diff(t)])
u_vect = sympy.Matrix([u(t), d(t)])
y_vect = x_vect



In [16]:

    
f_vect = sympy.Matrix([theta(t).diff(t), eom_theta_dd])
f_vect









    Out[16]:





$$\left[\begin{matrix}\frac{d}{d t} \theta{\left (t \right )}\\\frac{1}{l^{2} m} \left(g l m \sin{\left (\theta{\left (t \right )} \right )} + d{\left (t \right )} + u{\left (t \right )}\right)\end{matrix}\right]$$



In [17]:

    
A = f_vect.jacobian(x_vect)
B = f_vect.jacobian(u_vect)
C = y_vect.jacobian(x_vect)
D = y_vect.jacobian(u_vect)
(A,B,C,D)









    Out[17]:





$$\begin{pmatrix}\left[\begin{matrix}0 & 1\\\frac{g}{l} \cos{\left (\theta{\left (t \right )} \right )} & 0\end{matrix}\right], & \left[\begin{matrix}0 & 0\\\frac{1}{l^{2} m} & \frac{1}{l^{2} m}\end{matrix}\right], & \left[\begin{matrix}1 & 0\\0 & 1\end{matrix}\right], & \left[\begin{matrix}0 & 0\\0 & 0\end{matrix}\right]\end{pmatrix}$$

Kane's Method

Kane's method is a systematic way of deriving the equation of motion for a complicated possibly containing complicated constraints. It is overkill to apply it to this problem, but we will use it to introduce the method.



In [18]:

    
Q = sympy.Symbol('Q')
force_moment_list = [(point_p, weight), (frame_b, u(t)*frame_b.z)]
body_list = [particle_a]
q_ind = [theta(t)] # generalized coordinates
u_ind = [Q(t)] # generalized speeds
kd_eqs = [Q(t) - theta(t).diff(t)] # kinematic differential equations
km = me.KanesMethod(frame_i, q_ind, u_ind, kd_eqs=kd_eqs)
km.kanes_equations(force_moment_list, body_list)









    Out[18]:





$$\begin{pmatrix}\left[\begin{matrix}g l m \sin{\left (\theta{\left (t \right )} \right )} + u{\left (t \right )}\end{matrix}\right], & \left[\begin{matrix}- l^{2} m \frac{d}{d t} Q{\left (t \right )}\end{matrix}\right]\end{pmatrix}$$



In [19]:

    
km_mass = km.mass_matrix_full
km_mass_inv= km_mass.inv()

km_forcing_full = km.forcing_full
km_linear = km.linearize()
km_rhs = km.rhs()
(km_mass, km_mass_inv, km_forcing_full)









    Out[19]:





$$\begin{pmatrix}\left[\begin{matrix}1 & 0\\0 & l^{2} m\end{matrix}\right], & \left[\begin{matrix}1 & 0\\0 & \frac{1}{l^{2} m}\end{matrix}\right], & \left[\begin{matrix}Q{\left (t \right )}\\g l m \sin{\left (\theta{\left (t \right )} \right )} + u{\left (t \right )}\end{matrix}\right]\end{pmatrix}$$



In [20]:

    
(km_linear, km_rhs)









    Out[20]:





$$\begin{pmatrix}\begin{pmatrix}\left[\begin{matrix}0 & 1\\g l m \cos{\left (\theta{\left (t \right )} \right )} & 0\end{matrix}\right], & \left[\begin{matrix}0\\1\end{matrix}\right], & \left[\begin{matrix}u{\left (t \right )}\end{matrix}\right]\end{pmatrix}, & \left[\begin{matrix}Q{\left (t \right )}\\\frac{1}{l^{2} m} \left(g l m \sin{\left (\theta{\left (t \right )} \right )} + u{\left (t \right )}\right)\end{matrix}\right]\end{pmatrix}$$



In [21]:

    
km_A = km_mass_inv*km_linear[0]
km_B = km.mass_matrix_full.inv()*km.linearize()[1]
km_C = sympy.zeros(2)
km_D = sympy.zeros(2,1)
km_x = sympy.Matrix([theta(t), Q(t)])
km_u = sympy.Matrix([u(t)])
km_A, km_B, km_C, km_D, km_x









    Out[21]:





$$\begin{pmatrix}\left[\begin{matrix}0 & 1\\\frac{g}{l} \cos{\left (\theta{\left (t \right )} \right )} & 0\end{matrix}\right], & \left[\begin{matrix}0\\\frac{1}{l^{2} m}\end{matrix}\right], & \left[\begin{matrix}0 & 0\\0 & 0\end{matrix}\right], & \left[\begin{matrix}0\\0\end{matrix}\right], & \left[\begin{matrix}\theta{\left (t \right )}\\Q{\left (t \right )}\end{matrix}\right]\end{pmatrix}$$



In [22]:

    
km_forcing_full









    Out[22]:





$$\left[\begin{matrix}Q{\left (t \right )}\\g l m \sin{\left (\theta{\left (t \right )} \right )} + u{\left (t \right )}\end{matrix}\right]$$

We can also solve for the equilibrium conditions using the matrices from Kane's method.



In [23]:

    
sympy.solve(km_mass_inv*km_forcing_full, [Q(t), u(t)])









    Out[23]:





$$\begin{Bmatrix}Q{\left (t \right )} : 0, & u{\left (t \right )} : - g l m \sin{\left (\theta{\left (t \right )} \right )}\end{Bmatrix}$$

Lagrangian Mechanics



In [24]:

    
L = me.Lagrangian(frame_i, particle_a)
L









    Out[24]:





$$\frac{l^{2} m}{2} \left(\frac{d}{d t} \theta{\left (t \right )}\right)^{2}$$



In [25]:

    
lm = me.LagrangesMethod(L, [theta(t)], forcelist=force_moment_list, frame=frame_i)
lm.form_lagranges_equations()









    Out[25]:





$$\left[\begin{matrix}- g l m \sin{\left (\theta{\left (t \right )} \right )} + l^{2} m \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )} - u{\left (t \right )}\end{matrix}\right]$$

For this simple problem the Lagrangian method is more direct than Kane's method, but both give the same result.



In [26]:

    
lm.rhs(), km.rhs()









    Out[26]:





$$\begin{pmatrix}\left[\begin{matrix}\frac{d}{d t} \theta{\left (t \right )}\\\frac{1}{l^{2} m} \left(g l m \sin{\left (\theta{\left (t \right )} \right )} + u{\left (t \right )}\right)\end{matrix}\right], & \left[\begin{matrix}Q{\left (t \right )}\\\frac{1}{l^{2} m} \left(g l m \sin{\left (\theta{\left (t \right )} \right )} + u{\left (t \right )}\right)\end{matrix}\right]\end{pmatrix}$$

Linearization

Given the jacobian, we can linearize the dynamics at the equilibrium conditions. We will linearize about $\theta = 0$, the pendulum is inverted.



In [27]:

    
equil = {theta(t):0}
equil.update(values)
ss_eq = [ np.array(np.array(mat.subs(equil)), dtype=float) for mat in [A,B,C,D]]
sys = control.StateSpace(ss_eq[0], ss_eq[1], ss_eq[2], ss_eq[3])

Here we see that the system is unstable.



In [28]:

    
tout, xout, yout = control.forced_response(sys, X0=[0, 0.001], T=np.linspace(0,1))



In [29]:

    
plt.plot(tout, yout.T);
plt.legend([r'$\theta$', r'$\dot{\theta}$'], loc='best');
plt.show()

Control Design

Classical Frequency Domain Design



In [30]:

    
control.ss2tf(sys)









    Out[30]:





Input 1 to output 1:
      100
---------------
s^2 + -0 s - 98

Input 1 to output 2:
     100 s
---------------
s^2 + -0 s - 98

Input 2 to output 1:
      100
---------------
s^2 + -0 s - 98

Input 2 to output 2:
     100 s
---------------
s^2 + -0 s - 98

Looking at the root locus plot we see that the system is marginally stable at high gains as both poles approach the real axis. The system must be stabilized with derivative gain to pull the poles to the left hand side of the complex plane.



In [31]:

    
control.root_locus(sys[0,0], kvect=np.linspace(0,1,1000));

The bode plot shows that the phase margin is zero and that the system is unstable and we must increase the phase margin to around 70 degress/ gain margin of 10 dB to make the system robust to disturbances.



In [32]:

    
control.bode_plot(sys[0,0], omega=np.logspace(0, 2), );

We choose a PD type controller because we know that a zero must be added to the left hand plane from observing the root locus plot. We implement the PD controller with proportional gain from the $\theta$ and $\dot{\theta}$ plant states to avoid noise due to numerical differentiation. The gains were iteratively changed until the desired bandwidth and phase/gain margin were achieved.



In [33]:

    
kp_th = 2
kp_thd = 0.15
contr = [kp_th, kp_thd]
contr









    Out[33]:





$$\begin{bmatrix}2, & 0.15\end{bmatrix}$$



In [34]:

    
tfm = control.ss2tf(sys)
tfm









    Out[34]:





Input 1 to output 1:
      100
---------------
s^2 + -0 s - 98

Input 1 to output 2:
     100 s
---------------
s^2 + -0 s - 98

Input 2 to output 1:
      100
---------------
s^2 + -0 s - 98

Input 2 to output 2:
     100 s
---------------
s^2 + -0 s - 98



In [35]:

    
closed_loop = sys[:,0].feedback(contr)
prescaler = 1.0/np.linalg.norm(closed_loop.horner(0))
closed_loop *= prescaler
open_loop = prescaler*contr*sys



In [36]:

    
control.root_locus(closed_loop[0,0], kvect=np.linspace(0,1,100));



In [37]:

    
control.bode_plot(open_loop[0,0], omega=np.logspace(-1, 2), );



In [38]:

    
gm, pm, sm, gc, pc, sc = control.stability_margins(open_loop[0,0])
gm, pm, sm, gc, pc, sc









    Out[38]:





(None,
 45.910447258290475,
 0.90924251168982306,
 13.763953660529673,
 None,
 4.2265522274077174)



In [39]:

    
control.bode_plot(closed_loop[0,0], omega=np.logspace(-1, 2), );



In [40]:

    
closed_loop.damp()









    Out[40]:





(array([ 10.09950494,  10.09950494]),
 array([ 0.74261066,  0.74261066]),
 array([-7.5+6.76387463j, -7.5-6.76387463j]))



In [41]:

    
tout, yout, xout = control.forced_response(closed_loop, X0=[1, 1], T=np.linspace(0,1))
plt.plot(tout, yout.T);

Linear Quadratic Regulator Design



In [42]:

    
K, S, E = control.lqr(sys.A,sys.B[:,0],np.diag([1, 0]), 1*np.eye(1))
contr_lqr = K
contr_lqr









    Out[42]:





array([[ 2.38014285,  0.21818079]])



In [43]:

    
closed_loop_lqr = sys[:,0].feedback(contr_lqr)
prescaler_lqr = 1.0/np.linalg.norm(closed_loop_lqr.horner(0))
closed_loop_lqr *= prescaler_lqr
open_loop_lqr = sys[:,0]*contr_lqr



In [44]:

    
control.bode_plot(open_loop_lqr[0,0], omega=np.logspace(-1, 2), );



In [45]:

    
gm, pm, sm, gc, pc, sc = control.stability_margins(open_loop_lqr[0,0])
gm, pm, sm, gc, pc, sc









    Out[45]:





(None, 0.0, 0.0, 11.832763201615128, None, 11.832763201615132)



In [46]:

    
control.bode_plot(closed_loop_lqr[0,0], omega=np.logspace(-1, 2), );



In [47]:

    
closed_loop_lqr.damp()









    Out[47]:





(array([ 11.8327632,  11.8327632]),
 array([ 0.92193508,  0.92193508]),
 array([-10.90903949+4.58335494j, -10.90903949-4.58335494j]))



In [48]:

    
tout, yout, xout = control.forced_response(closed_loop_lqr, X0=[1, 1], T=np.linspace(0,1,1000))
plt.plot(tout, yout.T);

Nonlinear Simulation



In [49]:

    
f = sympy.lambdify((theta(t), Q(t), u(t)), km_rhs.subs(values))



In [50]:

    
u_eq.subs(values)









    Out[50]:





$$- d{\left (t \right )} - 0.98 \sin{\left (\theta{\left (t \right )} \right )}$$



In [51]:

    
feed_forward = sympy.lambdify(theta(t), (u_eq).subs(values).subs(d(t),0))
feed_forward(1)









    Out[51]:





$$-0.824641565112$$



In [52]:

    
np.array(contr).dot(np.array([1,1])) + feed_forward(1)









    Out[52]:





$$1.32535843489$$



In [53]:

    
def simulate(contr, tf=1.0, dt=1.0/1000.0, dt_cont= 1.0/50.0, x0=[1,1]):
    data = {}
    sim = scipy.integrate.ode(lambda t,x,u: f(x[0],x[1],u))
    sim.set_integrator('dopri5')
    sim.set_initial_value(x0)
    n_t = int(tf/dt)
    data['x'] = np.zeros((n_t, 2))
    data['u'] = np.zeros((n_t, 2))
    data['t'] = np.zeros(n_t)
    i = 0
    t_last_control = 0
    u_i = contr(x0)
    while sim.successful() and i < n_t:
        x_i = sim.y
        data['x'][i,:] = x_i
        data['u'][i,:] = u_i
        data['t'][i] = sim.t
        if (sim.t - t_last_control) >= dt_cont:
            t_last_control = sim.t
            u_i = contr(x_i)
        sim.set_f_params(u_i)
        sim.integrate(sim.t + dt)
        i += 1
    return data



In [54]:

    
x = sympy.DeferredVector('x')
cont_lqr_f = lambda x: -np.array(contr_lqr).dot(x)



In [55]:

    
plt.figure(figsize=(15,5))
h = []
freq_list = [15, 11, 10]
for freq in freq_list:
    data = simulate(cont_lqr_f,  dt_cont=1.0/freq);
    plt.subplot(121)
    h.append(plt.plot(data['t'], np.rad2deg(data['x'][:,0])))
    plt.subplot(122)
    plt.plot(data['t'], data['u'])
    
plt.subplot(121)
plt.legend([h_i[0] for h_i in h ], ['{:g} Hz'.format(freq) for freq in freq_list], loc='best')
plt.title('control sampling time effect (output)')
plt.xlabel('t, sec')
plt.ylabel(r'$\theta$, deg')

plt.subplot(122)
plt.xlabel('t, sec')
plt.ylabel('u')
plt.title('control sampling time effect (control)');

Digital Control

From the nonlinear simulation, it is clear that we need to account for the control sampling if the control will be sampled below 20 Hz. We do this using digital control design techniques. First, we will discretize the state space system at 10 Hz.



In [56]:

    
dt = 1.0/10
sysd = scipy.signal.cont2discrete((sys.A, sys.B, sys.C, sys.D), dt)
sysd = control.ss(sysd[0], sysd[1], sysd[2], sysd[3], dt)
sysd









    Out[56]:





A = [[  1.531347     0.1171526 ]
 [ 11.48095456   1.531347  ]]

B = [[  0.54219082   0.54219082]
 [ 11.71525976  11.71525976]]

C = [[ 1.  0.]
 [ 0.  1.]]

D = [[ 0.  0.]
 [ 0.  0.]]

dt = 0.1

Now we use the discrete algebraic ricatti equation to solve for the LQR gain for the discrete controller.



In [57]:

    
Q = np.diag([1, 0])
R = np.eye(1)
X, L, G1 = control.dare(sysd.A, sysd.B[:,0], Q, R)
X = scipy.linalg.solve_discrete_are(sysd.A, sysd.B[:,0], Q, R)
G = np.linalg.inv(R + sysd.B[:,0].T.dot(X).dot(sysd.B[:,0])).dot(sysd.B[:,0].T).dot(X).dot(sysd.A)
contr_lqrd = G
cont_lqrd_f = lambda x: -contr_lqrd.dot(x)

Why doesn't this give the same answer? It seems that python control might have a bug here, most of the number are the same except for the diagonal.



In [58]:

    
X2, L2, G2 = control.dare(sysd.A, sysd.B[:,0], Q, R)
print X
print X2









    



[[ 3.87377252  0.25714186]
 [ 0.25714186  0.02409592]]
[[-2.87377252  0.25714186]
 [ 0.25714186 -0.02409592]]



In [59]:

    
np.array(contr_lqr).dot(x[0])









    Out[59]:





array([[2.38014284985497*x[0], 0.218180789706838*x[0]]], dtype=object)

Now we compare the new discrete and previously designed continuous LQR gains.



In [60]:

    
plt.figure(figsize=(15,5))

plt.subplot(121)
plt.title('discrete vs. continuous LQR at 10 Hz (output)')
plt.xlabel('t, sec')
plt.ylabel(r'$\theta$, deg')
data_lqr = simulate(cont_lqr_f, dt_cont=dt, tf=2);
hc = plt.plot(data_lqr['t'], np.rad2deg(data_lqr['x'][:,0]));

plt.subplot(122)
plt.title('discrete vs. continuous LQR at 10 Hz (control)')
plt.xlabel('t, sec')
plt.ylabel('u')
hcc = plt.plot(data_lqr['t'], data_lqr['u']);

plt.subplot(121)
data_lqrd = simulate(cont_lqrd_f, dt_cont=dt, tf=2);
hd = plt.plot(data_lqrd['t'], np.rad2deg(data_lqrd['x'][:,0]));
plt.legend([hd[0], hc[0]], ['discrete','continuous']);

plt.subplot(122)
hdc = plt.plot(data_lqrd['t'], data_lqrd['u'])

Dynamic Inversion Control

Dynamic inversion control is useful for nonlinear systems that operate around many different equilibirum condiions. Dynamic inversion control allows you to design one controller that will work for all equilbirum conditoins, but it may saturate actuators or use more control effort to follow the desired dynamics.

Here we will make the pendulum follow the dynamics given by the natural frequency and damping ratio of a second order systems.



In [61]:

    
s, zeta, wn, Theta = sympy.symbols('s, zeta, \omega_n, Theta')



In [62]:

    
char_poly = Theta*(s**2 + 2*zeta*wn*s + wn**2)
char_poly









    Out[62]:





$$\Theta \left(\omega_{n}^{2} + 2 \omega_{n} s \zeta + s^{2}\right)$$



In [63]:

    
char_poly_theta = char_poly.expand().subs({Theta*s**2: theta(t).diff(t,2), Theta*s: theta(t).diff(t), Theta: theta(t)})
char_poly_theta









    Out[63]:





$$\omega_{n}^{2} \theta{\left (t \right )} + 2 \omega_{n} \zeta \frac{d}{d t} \theta{\left (t \right )} + \frac{d^{2}}{d t^{2}}  \theta{\left (t \right )}$$



In [64]:

    
theta_dd_des = sympy.solve(char_poly_theta, theta(t).diff(t,2))[0].expand()
theta_dd_des









    Out[64]:





$$- \omega_{n}^{2} \theta{\left (t \right )} - 2 \omega_{n} \zeta \frac{d}{d t} \theta{\left (t \right )}$$



In [65]:

    
eom_des = eom.subs({theta(t).diff(t,2): theta_dd_des.subs({wn:10, zeta:0.8})})
eom_des









    Out[65]:





$$g l m \sin{\left (\theta{\left (t \right )} \right )} - l^{2} m \left(- 100 \theta{\left (t \right )} - 16.0 \frac{d}{d t} \theta{\left (t \right )}\right) + d{\left (t \right )} + u{\left (t \right )}$$



In [66]:

    
dyn_inv_f = sympy.solve(eom_des, u(t))[0]
dyn_inv_f









    Out[66]:





$$- g l m \sin{\left (\theta{\left (t \right )} \right )} - 100.0 l^{2} m \theta{\left (t \right )} - 16.0 l^{2} m \frac{d}{d t} \theta{\left (t \right )} - d{\left (t \right )}$$



In [67]:

    
cont_dyn_inv_f  = sympy.lambdify(x, dyn_inv_f.subs(values).subs({theta(t): x[0], theta(t).diff(t): x[1], d(t):0}))



In [68]:

    
plt.figure(figsize=(15,5))

plt.subplot(121)
plt.title('dynamic inversion controller vs. discrete LQR (output)')
plt.xlabel('t, sec')
plt.ylabel(r'$\theta$, deg')
data_dyn_inv = simulate(cont_dyn_inv_f,  dt_cont=1.0/10, tf=2);
h_dyn_inv = plt.plot(data_dyn_inv['t'], np.rad2deg(data_dyn_inv['x'][:,0]));
h_lqrd = plt.plot(data_lqrd['t'], np.rad2deg(data_lqrd['x'][:,0]));
plt.legend([h_dyn_inv[0], h_lqrd[0]], ['dynamic inversion', 'discrete LQR'])

plt.subplot(122)
plt.title('(control)')
plt.plot(data_dyn_inv['t'], data_dyn_inv['u'])
plt.plot(data_lqrd['t'], data_lqrd['u'])









    Out[68]:





[<matplotlib.lines.Line2D at 0x521cd50>,
 <matplotlib.lines.Line2D at 0x5219e90>]

Dynamical inversion doesn't work perfectly here because we are stil sampling the control at a slow rate.

Inverted Pendulum Controller Animation



In [69]:

    
def pend_anim(data_list, data_names, tf=1.0, fps=20, rate=1.0):
    fig = plt.figure(figsize=(10,5))
    ax = fig.gca()
    def update(i):
        i_fig = 1
        ax.cla()
        h = []
        colors = itertools.cycle(['b', 'g', 'r', 'c', 'm', 'y', 'k'])
        for data in data_list:
            theta = data['x'][50*i*rate,0]
            x = -1.0*np.sin(theta)
            y = 1.0*np.cos(theta)
            #ax = plt.subplot(1,3,i_fig)
            ax.set_aspect('equal')
            ax.axis(1.1*np.array([-1, 1, -1, 1]))
            h.append(ax.plot(x, y, 'ro', markersize=10, c=colors.next())[0])
            ax.plot([0, x], [0, y], 'k-')
            i_fig += 1
        ax.legend(h, data_names, loc='lower left')
    return matplotlib.animation.FuncAnimation(plt.gcf(), update, frames=int(tf*fps/rate))



In [70]:

    
pend_anim([data_lqr, data_lqrd, data_dyn_inv], ['LQR', 'discrete LQR', 'dynamic inversoin'], rate=0.2)









    Out[70]:

Estimating Disturbance Envelope



In [72]:

    
closed_loop_lqr.A









    Out[72]:





matrix([[   0.        ,    1.        ],
        [-140.01428499,  -21.81807897]])



In [73]:

    
closed_loop_lqr.B









    Out[73]:





matrix([[  0.],
        [100.]])



In [74]:

    
closed_loop_lqr.C









    Out[74]:





matrix([[ 1.40014285,  0.        ],
        [ 0.        ,  1.40014285]])



In [ ]: