{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In this code example, we'll apply LQR feedback control to stabilize a 3D quadrotor at a point."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"collapsed": false
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Populating the interactive namespace from numpy and matplotlib\n",
"Initializing flashlight v0.0.1\n",
"flashlight.quadrotor_3d: Constructing sympy symbols...\n",
"flashlight.quadrotor_3d: Finished constructing sympy symbols (0.008 seconds).\n",
"flashlight.quadrotor_3d: Loading sympy modules...\n",
"flashlight.quadrotor_3d: Finished loading sympy modules (0.026 seconds).\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"/Users/mike/Code/GitHub/flashlight/code/lib/flashlight/transformations.py:1888: UserWarning: failed to import module _transformations\n",
" warnings.warn(\"failed to import module %s\" % name)\n"
]
}
],
"source": [
"%pylab inline\n",
"from pylab import *\n",
"\n",
"import control\n",
"import scipy.integrate\n",
"\n",
"import path_utils\n",
"path_utils.add_relative_to_current_source_file_path_to_sys_path(\"../../lib\")\n",
"\n",
"import flashlight.ipython_display_utils as ipython_display_utils\n",
"import flashlight.quadrotor_3d as quadrotor_3d"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"As in our previous 2D LQR example, we linearize the quadrotor dynamics about hovering-at-the-origin, and compute the optimal control matrix `K`. Next, we set up our simulation callback function to compute the appropriate feedback control forces and compute the time derivative of our quadrotor state. Again, we should include a bit of extra logic here to handle the fact that the angular dimensions of our state space wrap around. But we omit this detail for simplicity. Finally, we specify the initial state of our simulation to have a large initial velocity, to simulate a large unexpected disturbance (e.g., a gust of wind) occuring at the beginning of the simulation.\n",
"\n",
"Note that the flashlight conventions for ordering the entries in our state and control vectors are as follows.\n",
"\n",
"```\n",
"x = matrix( [ p_z, p_y, p_x, theta, psi, phi, p_z_dot, p_y_dot, p_x_dot, theta_dot, psi_dot, phi_dot ] ).T\n",
"u = matrix( [ u_front_right, u_rear_right, u_rear_left, u_front_left ] ).T\n",
"```\n",
"\n",
"Roughly speaking, `p_z`, `p_y`, and `p_x` specify the quadrotor's position; `theta` is the quadrotor's pitch angle (i.e., the rotation about the `z` axis); `psi` is the quadrotor's yaw angle (i.e., the rotation about the `y` axis); and `phi` is the quadrotor's roll angle (i.e., the rotation about the `x` axis). The remaining values in our state vector are the time derivatives. Note also that the quadrotor's positive `x`, `y`, and `z` axes are shown as the red, green, and blue lines extending out from the quadrotor in the animation below. We apply the Euler angle rotations to the rotating frame, applying yaw (`psi`) first, then pitch (`theta`), then roll (`phi`). For more details on the Euler angle conventions and notation we adopt throughout flashlight, see [1].\n",
"\n",
"#### References\n",
"\n",
"[1] Niels Joubert, Mike Roberts, Anh Truong, Floraine Berthouzoz, Pat Hanrahan. An Interactive Tool for Designing Quadrotor Camera Shots. _ACM Transactions on Graphics 34(6) (SIGGRAPH Asia 2015)_."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"collapsed": false
},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/Users/mike/Library/Enthought/Canopy_64bit/User/lib/python2.7/site-packages/traits/has_traits.py:1536: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.\n",
" setattr( self, name, value )\n"
]
},
{
"data": {
"text/html": [
"\n",
" \n",
" "
],
"text/plain": [
""
]
},
"execution_count": 2,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"m = quadrotor_3d.m\n",
"g = quadrotor_3d.g\n",
"\n",
"x_star = matrix([0,0,0,0,0,0,0,0,0,0,0,0]).T\n",
"u_star = matrix([m*g/4.0,m*g/4.0,m*g/4.0,m*g/4.0]).T\n",
"\n",
"Q = diag([1,1,1,1,1,1,1,1,1,1,1,1])\n",
"R = diag([1,1,1,1])\n",
"\n",
"A, B = quadrotor_3d.compute_df_dx_and_df_du(x_star, u_star)\n",
"K, S, E = control.lqr(A, B, Q, R)\n",
"\n",
"x_disturbance = matrix([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, 5.0, 5.0, -2.0*pi, 0.0, 0.0]).T\n",
"x_0 = (x_star + x_disturbance).A1\n",
"\n",
"def compute_x_dot(x_t, t):\n",
"\n",
" x_t = matrix(x_t).T \n",
" x_bar_t = x_t - x_star\n",
" u_bar_t = -K*x_bar_t\n",
" u_t = u_bar_t + u_star\n",
" x_dot_t = quadrotor_3d.compute_x_dot(x_t, u_t).A1\n",
" \n",
" return x_dot_t\n",
"\n",
"num_timesteps_sim = 200\n",
"\n",
"t_begin = 0.0\n",
"t_end = 10.0\n",
"t_sim = linspace(t_begin, t_end, num_timesteps_sim)\n",
"x_sim = scipy.integrate.odeint(compute_x_dot, x_0, t_sim)\n",
"\n",
"quadrotor_3d.draw(t_sim, x_sim)\n",
"# quadrotor_3d.draw(t_sim, x_sim, savefig=True, out_dir=\"data/09\", out_file=\"00.mp4\")\n",
"ipython_display_utils.get_inline_video(\"data/09/00.mp4\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 2",
"language": "python",
"name": "python2"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 2
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython2",
"version": "2.7.11"
}
},
"nbformat": 4,
"nbformat_minor": 0
}