{ "cells": [ { "cell_type": "markdown", "metadata": { "collapsed": false }, "source": [ "In this code example, we'll apply time-varying LQR feedback control to stabilize a 2D quadrotor along a trajectory." ] }, { "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.005 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.interpolate_utils as interpolate_utils\n", "import flashlight.quadrotor_2d as quadrotor_2d\n", "import flashlight.spline_utils as spline_utils" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We begin by computing our intended trajectory exactly as in the previous examples." ] }, { "cell_type": "code", "execution_count": 2, "metadata": { "collapsed": false }, "outputs": [ { "data": { "image/png": "iVBORw0KGgoAAAANSUhEUgAAATYAAADUCAYAAAAWRw+UAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nlFK2o8GmlLIdDTallO38fwVOJfG1O1JIAAAAAElFTkSuQmCC\n", "text/plain": [ "" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "T_y = matrix([0,1,2,3]).T.A\n", "T_x = matrix([0,1,2,3]).T.A\n", "T = c_[T_y, T_x]\n", "P_y = matrix([0,3,1,4]).T.A\n", "P_x = matrix([0,3,7,10]).T.A\n", "P = c_[P_y, P_x]\n", "\n", "num_samples = 200\n", "\n", "C, T, sd = \\\n", " spline_utils.compute_minimum_variation_nonlocal_interpolating_b_spline_coefficients(\n", " P, T, degree=7, lamb=[0,0,0,1,0])\n", " \n", "P_eval, T_eval, dT = \\\n", " spline_utils.evaluate_minimum_variation_nonlocal_interpolating_b_spline(\n", " C, T, sd, num_samples=num_samples)\n", "\n", "T_s = matrix([0.0,1.2,1.8,3.0]).T.A\n", "P_s = matrix([0.0,0.3,0.7,1.0]).T.A\n", "\n", "C_s, T_s, sd_s = \\\n", " spline_utils.compute_minimum_variation_nonlocal_interpolating_b_spline_coefficients(\n", " P_s, T_s, degree=7, lamb=[0,0,0,1,0])\n", " \n", "P_s_eval, T_s_eval, dT_s = \\\n", " spline_utils.evaluate_minimum_variation_nonlocal_interpolating_b_spline(\n", " C_s, T_s, sd_s, num_samples=num_samples)\n", "\n", "t = linspace(0.0,10.0,num_samples)\n", "s_spline = P_s_eval\n", "\n", "P_eval_spline, t_spline, P_eval_cum_length, t_norm = curve_utils.reparameterize_curve(P_eval, s_spline)\n", "\n", "t_begin = t[0]\n", "t_end = t[-1]\n", "num_timesteps = num_samples\n", "\n", "p = P_eval_spline\n", "dt = (t_end-t_begin) / (num_timesteps-1)\n", "\n", "q_qdot_qdotdot = quadrotor_2d.compute_state_space_trajectory_and_derivatives(p, dt)\n", "u = quadrotor_2d.compute_control_trajectory(q_qdot_qdotdot)\n", "\n", "figsize(5,2)\n", "plot(t, u[:,0]); plot(t, u[:,1]);\n", "title(\"$\\\\mathbf{u}(t)$\\n\", fontsize=20); xlabel(\"$t$\", fontsize=20);" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Next, we set up our simulation callback function to compute the appropriate feedback control forces and compute the time derivative of our quadrotor state. Our time-varying LQR control strategy here is similar to the LQR control strategy in our previous example. However, in this example, our linearization point varies with time. Instead of linearizing about hovering-at-the-origin, we linearize about a point that moves along our intended trajectory. Notice how applying time-varying LQR completely eliminates the drift away from our intended trajectory, which was an issue in our previous code example. For more details on time-varying LQR, see [1].\n", "\n", "#### References\n", "\n", "[1] Russ Tedrake. Underactuated Robotics: Algorithms for Walking, Running, Swimming, Flying, and Manipulation (course notes for MIT 6.832). 2016. http://underactuated.mit.edu" ] }, { "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_nominal, q_nominal, qdot_nominal, qdotdot_nominal = \\\n", " quadrotor_2d.pack_state_space_trajectory_and_derivatives(q_qdot_qdotdot)\n", "\n", "u_nominal = u\n", "\n", "x_0 = x_nominal[0]\n", "\n", "Q = diag([1,1,1,1,1,1])\n", "R = diag([1,1])\n", "\n", "def compute_x_dot(x_t, t):\n", "\n", " x_t = matrix(x_t).T\n", " \n", " x_nominal_t = x_nominal_interp_func(clip(t, t_begin, t_end))\n", " u_nominal_t = u_nominal_interp_func(clip(t, t_begin, t_end))\n", " \n", " A_t, B_t = quadrotor_2d.compute_df_dx_and_df_du(x_nominal_t, u_nominal_t)\n", " K_t, S_t, E_t = control.lqr(A_t, B_t, Q, R)\n", " \n", " x_bar_t = x_t - x_nominal_t\n", " u_bar_t = -K_t*x_bar_t\n", " u_t = u_bar_t + u_nominal_t\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", "t_sim = linspace(t_begin, t_end, num_timesteps_sim)\n", "x_nominal_interp_func = interpolate_utils.interp1d_vector_wrt_scalar(t, x_nominal, kind=\"cubic\")\n", "u_nominal_interp_func = interpolate_utils.interp1d_vector_wrt_scalar(t, u_nominal, kind=\"cubic\")\n", "x_sim = scipy.integrate.odeint(compute_x_dot, x_0, t_sim)\n", "\n", "figsize(9,4)\n", "quadrotor_2d.draw(t_sim, x_sim, t_nominal=t, x_nominal=x_nominal, inline=True)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Finally, we can also 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. Notice how applying time-varying LQR successfully attracts the quadrotor back to our intended trajectory." ] }, { "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, 0.0, 5.0, 5.0, -4.0*pi]).T\n", "x_0 = (matrix(x_nominal[0]).T + x_disturbance).A1\n", "x_sim = scipy.integrate.odeint(compute_x_dot, x_0, t_sim)\n", "\n", "figsize(9,4)\n", "quadrotor_2d.draw(t_sim, x_sim, t_nominal=t, x_nominal=x_nominal, 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 }