Dr. Joshua Vaughan
joshua.vaughan@louisiana.edu
http://www.ucs.louisiana.edu/~jev9637/
Figure 1: The Simple, Planar Crane Model
This notebook simluates a the simple, planar crane model like the one shown in Figure 1. We'll be treating the trolley as the input to the pendulum system. This is fairily consistent with "real" cranes, where we can generally control the trolley location fairly well and are intersted in controlling the payload.
In this case, we can more explicitly draw the model like that in Figure 2. Because we're saying that we can exactly control $x(t)$, the mass of the trolley doesn't matter. We're really just controlling the pendulum connection point.
Figure 2: The "Trolley-input" Planar Crane Model
If we are treating the trolley as the input, the equation of motion for the payload angle is:
$ \quad \ddot{\theta} + \frac{g}{l}\theta = \frac{1}{l}\ddot{x} $
In order to simluate the system (in this case a single, second-order ODE), we need to write it as a system of first-order differential equations. To do so, let's define a state vector:
$\quad \mathbf{w} = \left[w_1 \quad w_2\right]^T = \left[\theta \quad \dot{\theta}\right]^T $
We can then write the system in the form:
$ \quad \dot{\mathbf{w}} = f(\mathbf{w},\ddot{x},t) $
So,
$ \quad \dot{\mathbf{w}} = \left[w_2, \ -\frac{g}{l}w_1 + \frac{1}{l}\ddot{x}\right] $
We'll start by simluating the system using a nummerical differential equation solver, which could be used for more complex and nonlinear systems. We'll then see how to simluate the system using some functions from the Python Control Systems Toolbox.
First, we need to import NumPy, the standard numerical toolbox we use, and matplotlib, the standard plotting library. Nearly all of our projects will import NumPy, and those with plotting will always import matplotlib.
import numpy as np
# We want out plots to show up inline with the notebook
%matplotlib inline
import matplotlib.pyplot as plt
# We need to import the ode solver
from scipy.integrate import odeint
def eq_of_motion(w, t, p):
"""
Defines the differential equations for the planar pendulum system.
Arguments:
w : vector of the state variables:
t : time
p : vector of the parameters:
"""
theta, theta_dot = w
m, l, Distance, StartTime, Amax, Vmax, Shaper = p
# Create sysODE = (theta', theta_dot')
sysODE = [theta_dot,
-g/l * theta + 1.0/l * x_ddot(t, p)]
return sysODE
def x_ddot(t, p):
"""
Defines the accel input to the system.
We'll make a call to our lab function accel_input()
Depending on the desired move distance, max accel, and max velocity, the input is either
bang-bang or bang-coast-bang
Arguments:
t : current time step
p : vector of parameters
"""
m, l, Distance, StartTime, Amax, Vmax, Shaper = p
x_ddot = accel_input(Amax,Vmax,Distance,StartTime,t,Shaper)
return x_ddot
# This cell includes functions to generate the acceleartion input
def accel_input(Amax,Vmax,Distance,StartTime,CurrTime,Shaper):
"""
# Original MATLAB/Octave premable
###########################################################################
# function [accel] = accel_input(Amax,Vmax,Distance,CurrTime,Shaper)
#
# Function returns acceleration at a given timestep based on user input
#
# Amax = maximum accel, assumed to besymmetric +/-
# Vmax = maximum velocity, assumed to be symmetric in +/-
# Distance = desired travel distance
# StartTime = Time command should begin
# CurrTime = current time
# Shaper = array of the form [Ti Ai] - matches output format of shaper functions
# in toolbox
# * If Shaper is empty, then unshaped is run
#
#
# Assumptions:
# * +/- maximums are of same amplitude
# * command will begin at StartTime (default = 0)
# * rest-to-rest bang-coast-bang move (before shaping)
#
# Created: 9/23/11 - Joshua Vaughan - vaughanje@gatech.edu
#
# Modified:
# 10/11/11
# * Added hard-coded shaping option - JEV (vaughanje@gatech.edu)
#
###########################################################################
#
#
# Converted to Python on 3/3/13 by Joshua Vaughan (joshua.vaughan@louisiana.edu)
#
# Modified:
# * 3/26/14 - Joshua Vaughan - joshua.vaughan@louisiana.edu
# - Updated some commenting, corrected typos
# - Updated numpy import as np
"""
# These are the times for a bang-coast-bang input
t1 = StartTime
t2 = (Vmax/Amax) + t1
t3 = (Distance/Vmax) + t1
t4 = (t2 + t3)-t1
end_time = t4
if len(Shaper) == 0:
# If no shaper is input, create an unshaped command
if t3 <= t2: # command should be bang-bang, not bang-coast-bang
t2 = np.sqrt(Distance/Amax)+t1
t3 = 2.0 * np.sqrt(Distance/Amax)+t1
end_time = t3
accel = Amax*(CurrTime > t1) - 2*Amax*(CurrTime > t2) + Amax*(CurrTime > t3)
else: # command is bang-coast-bang
accel = Amax*(CurrTime > t1) - Amax*(CurrTime > t2) - Amax*(CurrTime > t3) + Amax*(CurrTime > t4)
else: # create a shaped command
ts = np.zeros((9,1))
A = np.zeros((9,1))
# Parse Shaper parameters
for ii in range(len(Shaper)):
ts[ii] = Shaper[ii,0] # Shaper impulse times
A[ii] = Shaper[ii,1] # Shaper impulse amplitudes
# Hard-coded for now
# TODO: be smarter about constructing the total input - JEV - 10/11/11
accel = (A[0]*(Amax*(CurrTime > (t1+ts[0])) - Amax*(CurrTime > (t2+ts[0])) - Amax*(CurrTime > (t3+ts[0])) + Amax*(CurrTime > (t4+ts[0])))
+ A[1]*(Amax*(CurrTime > (t1+ts[1])) - Amax*(CurrTime > (t2+ts[1])) - Amax*(CurrTime > (t3+ts[1])) + Amax*(CurrTime > (t4+ts[1])))
+ A[2]*(Amax*(CurrTime > (t1+ts[2])) - Amax*(CurrTime > (t2+ts[2])) - Amax*(CurrTime > (t3+ts[2])) + Amax*(CurrTime > (t4+ts[2])))
+ A[3]*(Amax*(CurrTime > (t1+ts[3])) - Amax*(CurrTime > (t2+ts[3])) - Amax*(CurrTime > (t3+ts[3])) + Amax*(CurrTime > (t4+ts[3])))
+ A[4]*(Amax*(CurrTime > (t1+ts[4])) - Amax*(CurrTime > (t2+ts[4])) - Amax*(CurrTime > (t3+ts[4])) + Amax*(CurrTime > (t4+ts[4])))
+ A[5]*(Amax*(CurrTime > (t1+ts[5])) - Amax*(CurrTime > (t2+ts[5])) - Amax*(CurrTime > (t3+ts[5])) + Amax*(CurrTime > (t4+ts[5])))
+ A[6]*(Amax*(CurrTime > (t1+ts[6])) - Amax*(CurrTime > (t2+ts[6])) - Amax*(CurrTime > (t3+ts[6])) + Amax*(CurrTime > (t4+ts[6])))
+ A[7]*(Amax*(CurrTime > (t1+ts[7])) - Amax*(CurrTime > (t2+ts[7])) - Amax*(CurrTime > (t3+ts[7])) + Amax*(CurrTime > (t4+ts[7])))
+ A[8]*(Amax*(CurrTime > (t1+ts[8])) - Amax*(CurrTime > (t2+ts[8])) - Amax*(CurrTime > (t3+ts[8])) + Amax*(CurrTime > (t4+ts[8]))))
return accel
# Define the parameters for simluation
g = 9.81 # gravity (m/s^2)
l = 4.0 # cable length (m)
wn = np.sqrt(g / l) # natural frequency (rad/s)
# ODE solver parameters
abserr = 1.0e-9
relerr = 1.0e-9
max_step = 0.01
stoptime = 15.0
numpoints = 15001
# Create the time samples for the output of the ODE solver
t = np.linspace(0.,stoptime,numpoints)
# Initial conditions
theta_init = 0.0 # initial position
theta_dot_init = 0.0 # initial velocity
# Set up the parameters for the input function
Distance = 2.0 # Desired move distance (m)
Amax = 1.0 # acceleration limit (m/s^2)
Vmax = 0.35 # velocity limit (m/s)
StartTime = 0.5 # Time the y(t) input will begin
# Design and define an input Shaper
Shaper = [] # An empty shaper means no input shaping
# Pack the parameters and initial conditions into arrays
p = [g, l, Distance, StartTime, Amax, Vmax, Shaper]
x0 = [theta_init, theta_dot_init]
# Call the ODE solver.
response = odeint(eq_of_motion, x0, t, args=(p,), atol=abserr, rtol=relerr, hmax=max_step)
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
# Set the plot size - 3x2 aspect ratio is best
fig = plt.figure(figsize=(6,4))
ax = plt.gca()
plt.subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
# Change the axis units to serif
plt.setp(ax.get_ymajorticklabels(),family='serif',fontsize=18)
plt.setp(ax.get_xmajorticklabels(),family='serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
# Turn on the plot grid and set appropriate linestyle and color
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)
# Define the X and Y axis labels
plt.xlabel('Time (s)',family='serif',fontsize=22,weight='bold',labelpad=5)
plt.ylabel('Angle (deg)',family='serif',fontsize=22,weight='bold',labelpad=10)
plt.plot(t, response[:,0]*180/np.pi, linewidth=2, label=r'Angle')
# uncomment below and set limits if needed
plt.xlim(0,15)
# plt.ylim(0,10)
# Create the legend, then fix the fontsize
# leg = plt.legend(loc='upper right', fancybox=True)
# ltext = leg.get_texts()
# plt.setp(ltext,family='serif',fontsize=18)
# Adjust the page layout filling the page using the new tight_layout command
plt.tight_layout(pad=0.5)
# save the figure as a high-res pdf in the current folder
# It's saved at the original 6x4 size
# plt.savefig('Crane_AngleResponse.pdf')
fig.set_size_inches(9,6) # Resize the figure for better display in the notebook
We can also include the trolley location in the state vector. We know that how the position and velocity are related to the acceleration input. The new state vector becomes:
$\quad \mathbf{w} = \left[\theta \quad \dot{\theta} \quad x \quad \dot{x}\right]^T $
We can then write the system in the form:
$ \quad \dot{\mathbf{w}} = f(\mathbf{w},\ddot{x},t) $
So,
$ \quad \dot{\mathbf{w}} = \begin{bmatrix}w_2 \\ -\frac{g}{l}w_1 + \frac{1}{l}\ddot{x} \\ w_4 \\ \ddot{x}\end{bmatrix} $
The acceleration, $\ddot{x}(t)$, is still the input to the system. The function from above can be used unaltered. It's recopied below to avoid confusion, but note that it is generally bad practice to have functions defined in muliple places in a script or notebook.
def eq_of_motion_withTrolley(w, t, p):
"""
Defines the differential equations for the coupled spring-mass system.
Arguments:
w : vector of the state variables:
t : time
p : vector of the parameters:
"""
theta, theta_dot, x, x_dot = w
m, l, Distance, StartTime, Amax, Vmax, Shaper = p
# Create sysODE = (theta', theta_dot')
sysODE = [theta_dot,
-g/l * theta + 1.0/l * x_ddot(t, p),
x_dot,
x_ddot(t, p)]
return sysODE
def x_ddot(t, p):
"""
Defines the accel input to the system.
We'll make a call to our lab function accel_input()
Depending on the desired move distance, max accel, and max velocity, the input is either
bang-bang or bang-coast-bang
Arguments:
t : current time step
p : vector of parameters
"""
m, l, Distance, StartTime, Amax, Vmax, Shaper = p
x_ddot = accel_input(Amax,Vmax,Distance,StartTime,t,Shaper)
return x_ddot
We now need to specify the initial conditions for all the states, which include $x$ now. I've also respecified all the solver and system parameters for completeness. This is arguably bad practice, if the same parameters as above are being used. This also means that anytime I want to change a parameter, I would have to change it in mulitple places.
# Define the parameters for simluation
g = 9.81 # gravity (m/s^2)
l = 4.0 # cable length (m)
wn = np.sqrt(g / l) # natural frequency (rad/s)
# ODE solver parameters
abserr = 1.0e-9
relerr = 1.0e-9
max_step = 0.01
stoptime = 15.0
numpoints = 15001
# Create the time samples for the output of the ODE solver
t = np.linspace(0.0, stoptime, numpoints)
# Initial conditions
theta_init = 0.0 # initial angle (rad)
theta_dot_init = 0.0 # initial angular velocity (rad/s)
x_init = 0.0 # initial trolley position (m)
x_dot_init = 0.0 # initial trolley velocity (m/s)
# Set up the parameters for the input function
Distance = 2.0 # Desired move distance (m)
Amax = 1.0 # acceleration limit (m/s^2)
Vmax = 0.35 # velocity limit (m/s)
StartTime = 0.5 # Time the y(t) input will begin
# Design and define an input Shaper
Shaper = [] # An empty shaper means no input shaping
# Pack the parameters and initial conditions into arrays
p = [g, l, Distance, StartTime, Amax, Vmax, Shaper]
x0 = [theta_init, theta_dot_init, x_init, x_dot_init]
# Call the ODE solver.
response_withTrolley = odeint(eq_of_motion_withTrolley, x0, t, args=(p,), atol=abserr, rtol=relerr, hmax=max_step)
We can now parse the output of the ode solver (which now includes the trolley position) to plot the position of the trolley and payload. We know that the horizontal position of the payload is defined by:
$ \quad x_{paylaod} = x + l \sin{\theta} $
theta_resp = response_withTrolley[:,0]
theta_dot_resp = response_withTrolley[:,1]
x_resp = response_withTrolley[:,2]
x_dot_resp = response_withTrolley[:,3]
payload_position = x_resp + l * np.sin(theta_resp)
# Now, let's plot the horizontal position response
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
# Set the plot size - 3x2 aspect ratio is best
fig = plt.figure(figsize=(6,4))
ax = plt.gca()
plt.subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
# Change the axis units to serif
plt.setp(ax.get_ymajorticklabels(),family='serif',fontsize=18)
plt.setp(ax.get_xmajorticklabels(),family='serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
# Turn on the plot grid and set appropriate linestyle and color
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)
# Define the X and Y axis labels
plt.xlabel('Time (s)',family='serif',fontsize=22,weight='bold',labelpad=5)
plt.ylabel('Position (m)',family='serif',fontsize=22,weight='bold',labelpad=10)
plt.plot(t, x_resp, linewidth=2, linestyle='--', label=r'Trolley')
plt.plot(t, payload_position, linewidth=2, linestyle='-', label=r'Payload')
# uncomment below and set limits if needed
plt.xlim(0,15)
plt.ylim(0,3)
# Create the legend, then fix the fontsize
leg = plt.legend(loc='upper right', ncol = 2, fancybox=True)
ltext = leg.get_texts()
plt.setp(ltext,family='serif',fontsize=18)
# Adjust the page layout filling the page using the new tight_layout command
plt.tight_layout(pad=0.5)
# save the figure as a high-res pdf in the current folder
# It's saved at the original 6x4 size
# plt.savefig('Crane_Position_Response.pdf')
fig.set_size_inches(9,6) # Resize the figure for better display in the notebook
We can also simulate this system using the Python Control System Library. We'll first set up the state-space form of the equations of motion by writing our system of first-order ODEs in matrix form. Our system of first-order ODEs is:
$ \quad \dot{\mathbf{w}} = g(\mathbf{w}, \bar{u}, t) $
Expanding these, we have:
$ \quad \dot{\mathbf{w}} = \begin{bmatrix}w_2 \\ -\frac{g}{l}w_1 + \frac{1}{l}\ddot{x} \\ w_4 \\ \ddot{x}\end{bmatrix} $
The acceleration, $\ddot{x}(t)$, is still the input to the system.
$ \quad \mathbf{u} = \left[ \ddot{x} \right]$
We want to put this into the "normal" state-space form:
$ \quad \dot{\mathbf{w}} = A \mathbf{w} + B \bar{u} $
where A and B are matrices, and, as before, the state vector, $\mathbf{w}$, is:
$ \quad \mathbf{w} = \begin{bmatrix} \theta \\ \dot{\theta} \\ x \\ \dot{x} \end{bmatrix} $
*Note:* It is more common to see the state vector written as $\mathbf{x}$ instead of $\mathbf{w}$ as we've written here.
Looking at our equations of motion, we can then write:
$ \quad \dot{\mathbf{w}} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ -g/l & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{bmatrix} \mathbf{w} + \begin{bmatrix} 0 \\ 1/l \\ 0 \\ 1 \end{bmatrix} \mathbf{u} $
where:
$ \quad A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ -g/l & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{bmatrix} $
and:
$ \quad B = \begin{bmatrix} 0 \\ 1/l \\ 0 \\ 1 \end{bmatrix} $
Now that we've determined what the state-space matrices are, we can set up the system using the Control Systems Library. To do so, we'll first need to import the library:
import control
Then, we'll first define the matrices. In this case, we'll set the output matrix, C, to be include $\theta$, $x$, and well as the linarized prediction of the payload position $x - l\theta$.
A = np.array([[0, 1, 0, 0],
[-g/l, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
B = np.array([[0],[1/l],[0],[1]])
C = np.array([[1, 0, 0, 0], # theta
[0, 0, 1, 0], # x
[-l, 0, 1, 0]]) # x - l theta (paylod horiz position)
D = np.array([[0],[0],[0]])
Now, we can use the control.ss
method to create the system.
sys = control.ss(A, B, C, D)
As we saw above, the odeint
method of simluating the system needed all of the input to be defined in function form. However, the Control System Library needs the inputs to be arrays. So, we need to generate the arrays for our inputs, $\ddot{x}$.
In this case, since we already have the funcitonal form defined, we can use those funcitons to fill the needed array.
xddot_input = x_ddot(t, p)
t_resp, resp_out, states_out = control.forced_response(sys, t, xddot_input)
Now, let's plot this response as we did for the odeint
simulated version above.
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
fig = plt.figure(figsize=(6,4))
ax = plt.gca()
plt.subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
plt.setp(ax.get_ymajorticklabels(),family='serif',fontsize=18)
plt.setp(ax.get_xmajorticklabels(),family='serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)
plt.xlabel('Time (s)',family='serif',fontsize=22,weight='bold',labelpad=5)
plt.ylabel('Angle (deg)',family='serif',fontsize=22,weight='bold',labelpad=10)
# plt.ylim(-1.,1.)
# plot the response
plt.plot(t, resp_out[0,:], linewidth=2, linestyle = '-', label=r'$\theta$')
# leg = plt.legend(loc='upper right', fancybox=True)
# ltext = leg.get_texts()
# plt.setp(ltext,family='Serif',fontsize=16)
# Adjust the page layout filling the page using the new tight_layout command
plt.tight_layout(pad=0.5)
# If you want to save the figure, uncomment the commands below.
# The figure will be saved in the same directory as your IPython notebook.
# Save the figure as a high-res pdf in the current folder
# plt.savefig('planar_crane_angle.pdf')
fig.set_size_inches(9,6) # Resize the figure for better display in the notebook
As above, we can also plot the trolley and payload positions as a function of time. Since we defined one of our outputs to be the payload position, we can use it directly, rather than having to calculate it after the simluation like we have to do for the odeint
case.
# Now, let's plot the horizontal position response
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
# Set the plot size - 3x2 aspect ratio is best
fig = plt.figure(figsize=(6,4))
ax = plt.gca()
plt.subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
# Change the axis units to serif
plt.setp(ax.get_ymajorticklabels(),family='serif',fontsize=18)
plt.setp(ax.get_xmajorticklabels(),family='serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
# Turn on the plot grid and set appropriate linestyle and color
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)
# Define the X and Y axis labels
plt.xlabel('Time (s)',family='serif',fontsize=22,weight='bold',labelpad=5)
plt.ylabel('Position (m)',family='serif',fontsize=22,weight='bold',labelpad=10)
plt.plot(t, resp_out[1,:], linewidth=2, linestyle='--', label=r'Trolley')
plt.plot(t, resp_out[2,:], linewidth=2, linestyle='-', label=r'Payload')
# uncomment below and set limits if needed
plt.xlim(0,15)
plt.ylim(0,3)
# Create the legend, then fix the fontsize
leg = plt.legend(loc='upper right', ncol = 2, fancybox=True)
ltext = leg.get_texts()
plt.setp(ltext,family='serif',fontsize=18)
# Adjust the page layout filling the page using the new tight_layout command
plt.tight_layout(pad=0.5)
# save the figure as a high-res pdf in the current folder
# It's saved at the original 6x4 size
# plt.savefig('Crane_Position_Response.pdf')
fig.set_size_inches(9,6) # Resize the figure for better display in the notebook
The plot matches the one above, as it should.
The Control System Library also has several control design tools built in (as you might expect it to). Here, we'll design a Linear Quadratic Regulator (LQR) for this system
The form of the control law for LQR is:
$ \quad \mathbf{u} = -K \mathbf {w} $
where $K$ is a matrix containing gains. The gains multiple each state of the system via the state vector $\mathbf{w}$. Hence, this is a full-state feedback controller.
For this problem, since we have a nonzero desired state, we can redefine the control law to be:
$ \quad \mathbf{u} = -K \left( \mathbf{w} - \mathbf{w_d} \right)$
where $\mathbf{w_d}$ is the desired final state of the system, written as a vector. Luckily, this reformulation does not change the solution of the gain matrix $K$ at all. So, we can still use the Control System Library's built-in LQR solver.
The full derivation of the controller is beyond the scope of this notebook, but we do need to know that it is an (mathmeatically, not necessarily-practically) optimal controller. It is desined by picking the gain matrix to minimize a cost function of the form:
$ \quad J = \int_0^\infty \mathbf{w}^T Q\mathbf{w} + \mathbf{u}^T R \mathbf{u} + 2\mathbf{w}^T N \mathbf{u}\; dt $
For our purposes, let's ignore the $N$ matrix for now and concentrate on the $Q$ and $R$ matrices. These two matrices must be positive-semidefinite. The $Q$ matrix effectively penalizes states from deviating from the desired, regulated value. The $R$ matrix penalizes actuator effort. So, by changing the relative values within $Q$ we can value one state over another and by changing the ratio of values between $Q$ and $R$, we can balance performance and actuator effort.
As a simple choice, here let's set $Q$ to penalize $\theta$, $\dot{\theta}$ and $\dot{x}$, with slightly more emphasis on $\theta$.
Q = np.array([[ 100000, 0, 0, 0],
[ 0, 1000, 0, 0],
[ 0, 0, 1000, 0],
[ 0, 0, 0, 0]])
We'll simply set a low value for $R$, which will be a scalar value since there is only one input.
R = 0.01
Now, we can use the control.lqr
method of the Control System Library to design the controller by simply passing it the system, $Q$, and $R$ matrices.
K, _, closedLoopEigenvalues = control.lqr(A, B, Q, R)
For this problem, we want to regulate our states not about zero ut about some desired final state vector $\mathbf{w_d}$. To so do, we need to define an augmented state-space system to implement this input.
We saw that:
$ \quad \mathbf{u} = -K \left( \mathbf{w} - \mathbf{w_d} \right)$
and know that our system form is:
$ \quad \mathbf{\dot{w}} = A \mathbf{w} + B \mathbf{u} $
We can subsitute our control law for $\mathbf{u}$ into this equation to determine what the "new" matrices need to be:
$ \quad \mathbf{\dot{w}} = A \mathbf{w} - B K \left( \mathbf{w} - \mathbf{w_d} \right) $
We can now collect terms to find:
$ \quad \mathbf{\dot{w}} = (A - BK) \mathbf{w} + B K \mathbf{w_d} $
From this, we can use the solution from the "standard" regular problem to find $K$, we can define:
$ \quad \widehat{A} = A - BK $
and
$ \quad \widehat{B} = BK $
A_hat = A - B * K
B_hat = B * K
# Given the size change of the input matrix, we also need to to modifiy the D matrix
D_hat = np.zeros((3, 4))
sys_lqr = control.ss(A_hat, B_hat, C, D_hat)
We now need to define the desired state trajectory, $\mathbf{w_d}$.
# Set it to all zeros to start
w_d = np.zeros((4,len(t)))
# Now, fill the trolley position row with the desired final trolley position
x_d = 2.0
w_d[2,:] = x_d * np.ones((1,len(t)))
t_LQRresp, LQRresp_out, LQRstates_out = control.forced_response(sys_lqr, t, w_d, X0=[0,0,0,0])
# Now, let's plot the horizontal position response
# Make the figure pretty, then plot the results
# "pretty" parameters selected based on pdf output, not screen output
# Many of these setting could also be made default by the .matplotlibrc file
# Set the plot size - 3x2 aspect ratio is best
fig = plt.figure(figsize=(6,4))
ax = plt.gca()
plt.subplots_adjust(bottom=0.17,left=0.17,top=0.96,right=0.96)
# Change the axis units to serif
plt.setp(ax.get_ymajorticklabels(),family='serif',fontsize=18)
plt.setp(ax.get_xmajorticklabels(),family='serif',fontsize=18)
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
# Turn on the plot grid and set appropriate linestyle and color
ax.grid(True,linestyle=':',color='0.75')
ax.set_axisbelow(True)
# Define the X and Y axis labels
plt.xlabel('Time (s)',family='serif',fontsize=22,weight='bold',labelpad=5)
plt.ylabel('Position (m)',family='serif',fontsize=22,weight='bold',labelpad=10)
plt.plot(t, LQRresp_out[1,:], linewidth=2, linestyle='--', label=r'Trolley')
plt.plot(t, LQRresp_out[2,:], linewidth=2, linestyle='-', label=r'Payload')
# uncomment below and set limits if needed
plt.xlim(0,15)
# plt.ylim(0,3)
# Create the legend, then fix the fontsize
leg = plt.legend(loc='upper right', ncol = 2, fancybox=True)
ltext = leg.get_texts()
plt.setp(ltext,family='serif',fontsize=18)
# Adjust the page layout filling the page using the new tight_layout command
plt.tight_layout(pad=0.5)
# save the figure as a high-res pdf in the current folder
# It's saved at the original 6x4 size
# plt.savefig('Crane_Position_Response.pdf')
fig.set_size_inches(9,6) # Resize the figure for better display in the notebook
Code is licensed under a 3-clause BSD style license. See the licenses/LICENSE.md file.
Other content is provided under a Creative Commons Attribution-NonCommercial 4.0 International License, CC-BY-NC 4.0.
# This cell will just improve the styling of the notebook
# You can ignore it, if you are okay with the default sytling
from IPython.core.display import HTML
import urllib.request
response = urllib.request.urlopen("https://cl.ly/1B1y452Z1d35")
HTML(response.read().decode("utf-8"))