{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In this code example, we'll apply LQR feedback control to stabilize a 2D quadrotor at a point, taking care to handle angular wrapping correctly."
]
},
{
"cell_type": "code",
"execution_count": 1,
"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_2d: Constructing sympy symbols...\n",
"flashlight.quadrotor_2d: Finished constructing sympy symbols (0.006 seconds).\n",
"flashlight.quadrotor_2d: Loading sympy modules...\n",
"flashlight.quadrotor_2d: Finished loading sympy modules (0.006 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 matplotlib.animation\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.curve_utils as curve_utils\n",
"import flashlight.quadrotor_2d as quadrotor_2d\n",
"import flashlight.spline_utils as spline_utils\n",
"import flashlight.trig_utils as trig_utils"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We begin with our previous 2D LQR example, which does not exhibit any troublesome behavior due to angular wrapping. We will use this animation as a reference to verify that we are handling wrapping correctly."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 2,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"m = quadrotor_2d.m\n",
"g = quadrotor_2d.g\n",
"\n",
"x_star = matrix([0,0,0,0,0,0]).T\n",
"u_star = matrix([m*g/2.0,m*g/2.0]).T\n",
"\n",
"Q = diag([1,1,1,1,1,1])\n",
"R = diag([1,1])\n",
"\n",
"A, B = quadrotor_2d.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, 5.0, 5.0, -4.0*pi]).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_2d.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",
"figsize(6,4)\n",
"quadrotor_2d.draw(t_sim, x_sim, inline=True)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Next, we set our our initial state to be the same as in the code snippet above, but with an offset of `8*pi` applied to `theta`. `theta=8*pi` is the same initial orientation as `theta=0`, so if we handled angular wrapping correctly, we should see the same behavior as in the animation above. However, because we make no attempt to handle angular wrapping, we arrive at a quite different trajectory back to the goal state."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 3,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"x_disturbance = matrix([0.0, 0.0, 8.0*pi, 5.0, 5.0, -4.0*pi]).T\n",
"x_0 = (x_star + x_disturbance).A1\n",
"x_sim = scipy.integrate.odeint(compute_x_dot, x_0, t_sim)\n",
"\n",
"figsize(6,4)\n",
"quadrotor_2d.draw(t_sim, x_sim, inline=True)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Finally, we handle angular wrapping with the following strategy. At every simulation step, we update our goal state to include the value for `theta_goal` that is both equivalent to our original `theta_goal` value, and is _closest_ (in a radial sense) to the current value of `theta`. This strategy guarantees that we are never computing overly large control forces due to angular wrapping. From visual inspection, we see that this strategy yields the same trajectory back to the goal as the code snippet at the top of this example."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"collapsed": false
},
"outputs": [
{
"data": {
"text/html": [
""
],
"text/plain": [
""
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"x_disturbance = matrix([0.0, 0.0, 8.0*pi, 5.0, 5.0, -4.0*pi]).T\n",
"x_0 = (x_star + x_disturbance).A1\n",
"\n",
"p_star, theta_star, p_dot_star, theta_dot_star, q_star, q_dot_star = quadrotor_2d.unpack_state(x_star)\n",
"\n",
"def compute_x_dot(x_t, t):\n",
"\n",
" x_t = matrix(x_t).T\n",
" p_t, theta_t, p_dot_t, theta_dot_t, q_t, q_dot_t = quadrotor_2d.unpack_state(x_t)\n",
"\n",
" theta_star_hat_t = theta_t + trig_utils.compute_smallest_angular_diff(theta_t, theta_star) \n",
" x_star_hat_t, _, _ = quadrotor_2d.pack_state(p_star, theta_star_hat_t, p_dot_star, theta_dot_star)\n",
" \n",
" x_bar_t = x_t - x_star_hat_t\n",
" u_bar_t = -K*x_bar_t\n",
" u_t = u_bar_t + u_star\n",
" x_dot_t = quadrotor_2d.compute_x_dot(x_t, u_t).A1\n",
" \n",
" return x_dot_t\n",
"\n",
"x_sim = scipy.integrate.odeint(compute_x_dot, x_0, t_sim)\n",
"\n",
"figsize(6,4)\n",
"quadrotor_2d.draw(t_sim, x_sim, inline=True)"
]
}
],
"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
}