{ "metadata": { "name": "" }, "nbformat": 3, "nbformat_minor": 0, "worksheets": [ { "cells": [ { "cell_type": "heading", "level": 1, "metadata": {}, "source": [ "Introduction" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "In this last notebook, we will create a full state feedback controller that will allow the human model to balance. There isn't time to give a good overview of controls, but we will use a very common optimal control method to quickly create a controller for our system." ] }, { "cell_type": "heading", "level": 1, "metadata": {}, "source": [ "Setup" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Bring in the results from the previous notebooks." ] }, { "cell_type": "code", "collapsed": false, "input": [ "from solution.visualization import *" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will be working with numerical matrices so we will need several functions from NumPy." ] }, { "cell_type": "code", "collapsed": false, "input": [ "from numpy import array, zeros, eye, asarray, dot, rad2deg\n", "from numpy.linalg import inv" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will be simulating the model again with the controller in place and will make some plots of the state trajectories as before so bring in some useful functions from matplotlib. First enable inline plotting:" ] }, { "cell_type": "code", "collapsed": false, "input": [ "%matplotlib inline" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "from matplotlib.pyplot import plot, xlabel, ylabel, legend, rcParams" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "rcParams['figure.figsize'] = (14, 8)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Bring in some SymPy functions for simpilfication and printing:" ] }, { "cell_type": "code", "collapsed": false, "input": [ "from sympy import simplify\n", "from sympy.physics.vector import init_vprinting, vlatex" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "init_vprinting()" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will be displaying numerical matrices and the IPython magic `%precision` can be used to adjust how many decimal places are shown for nice compact printing." ] }, { "cell_type": "code", "collapsed": false, "input": [ "%precision 3" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 1, "metadata": {}, "source": [ "Linearization" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We current have symbolic equations of motion in this form:\n", "\n", "$$ \\mathbf{M}(\\mathbf{x}) \\dot{\\mathbf{x}} = \\mathbf{f}(\\mathbf{x}, \\mathbf{u}, t) $$\n", "\n", "where $\\mathbf{M}$ is the mass matrix, $\\mathbf{x}$ is the state vector made up of the generalized coordinates and speeds, and $\\mathbf{f}(\\mathbf{x}, \\mathbf{u})$ is a vector of non-linear functions of the states, $\\mathbf{x}$, and the specified inputs, $\\mathbf{u}$.\n", "\n", "This model is relatively easy to linearize and we will use a quick and dirty method to do so. In general, the non-linear equations of motion (for an unconstrained system) can be transformed into a linear form by expanding the Taylor series about small perturbations and truncating any non-linear terms. For this problem, this amounts to computing the [Jacobian](https://en.wikipedia.org/wiki/Jacobian) of the forcing vector, $\\mathbf{f}$. The linear equations take this form:\n", "\n", "$$ \\mathbf{M} \\Delta \\dot{\\mathbf{x}} = \\mathbf{f}_A \\Delta \\mathbf{x} + \\mathbf{f}_B \\Delta \\mathbf{u} $$\n", "\n", "where $\\mathbf{f}_A$ and $\\mathbf{f}_B$ are the Jacobians of $\\mathbf{f}$ with respect to the states and specified inputs, respectively:\n", "\n", "$$ \\mathbf{f}_A = \\left[ \\frac{\\delta \\mathbf{f} }{ \\delta x_1}, \\ldots, \\frac{\\delta \\mathbf{f} }{ \\delta x_n} \\right] $$\n", "\n", "$$ \\mathbf{f}_B = \\left[ \\frac{\\delta \\mathbf{f} }{ \\delta u_1}, \\ldots, \\frac{\\delta \\mathbf{f} }{ \\delta u_n} \\right] $$\n", "\n", "and $\\Delta \\mathbf{x}$ and $\\Delta \\mathbf{u}$ are _differential_ changes in $\\mathbf{x}$ and $\\mathbf{u}$ respectively.\n", "\n", "Once $\\mathbf{f}_A$ and $\\mathbf{f}_B$ are found, then the standard state space form of the linear equations can be formed by transforming the linear equations.\n", "\n", "$$ \\Delta \\dot{x} = \\mathbf{M}^{-1} \\mathbf{f}_A \\Delta \\mathbf{x} + \\mathbf{M}^{-1} \\mathbf{f}_B \\Delta \\mathbf{u} $$\n", "\n", "which can be rewritten as\n", "\n", "$$ \\Delta \\dot{x} = \\mathbf{A} \\Delta \\mathbf{x} + \\mathbf{B} \\Delta \\mathbf{u} $$\n", " \n", "where $\\mathbf{A}$ is the state matrix and $\\mathbf{B}$ is the input matrix." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The first step is to specify the equilbrium point about which to linearize. Our system has two unique equilibrium points, one when the person is upright, $\\mathbf{x} = [0, \\ldots, 0]$, and one when the person is upside down, $\\mathbf{x} = [\\pi, \\ldots, \\pi]$ (180 degrees). We are only concerned with the former because we want the model to \"stand up\"." ] }, { "cell_type": "code", "collapsed": false, "input": [ "equilibrium_point = zeros(len(coordinates + speeds))\n", "equilibrium_point" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We will use SymPy's `sub()` method to convert the linear equations of motion to numerical arrays, so we create two dictionaries containing the numerical values of the equilibrium point and the constants." ] }, { "cell_type": "code", "collapsed": false, "input": [ "equilibrium_dict = dict(zip(coordinates + speeds, equilibrium_point))\n", "equilibrium_dict" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 2, "metadata": {}, "source": [ "Exercise" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "As an exercise, create a `parameter_dict` that maps the symbolic constants to the numerical constants. We have variable names for each already available from the previous notebooks (`constants` and `numerical_constants`)." ] }, { "cell_type": "code", "collapsed": false, "input": [], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "## Solution\n", "%load exercise_solutions/n09_control_parameter-dict.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now we can compute the Jacobian of the forcing vector with respect to the states and the inputs using the `jacobian()` method." ] }, { "cell_type": "code", "collapsed": false, "input": [ "F_A = forcing_vector.jacobian(coordinates + speeds)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "First substitute in the equilibrium point values:" ] }, { "cell_type": "code", "collapsed": false, "input": [ "F_A = simplify(F_A.subs(equilibrium_dict))\n", "F_A" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now let's substitute in the numerical values for the constants and convert to a floating point NumPy array:" ] }, { "cell_type": "code", "collapsed": false, "input": [ "F_A = F_A.subs(parameter_dict)\n", "F_A = array(F_A.tolist(), dtype=float)\n", "print(F_A)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 2, "metadata": {}, "source": [ "Exercise" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Linearize the forcing vector with respect to the specified inputs by computing the correct Jacobian and creating a numerical array for $\\mathbf{f}_B$." ] }, { "cell_type": "code", "collapsed": false, "input": [], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "## Solution\n", "%load exercise_solutions/n09_control_linearize-forcing-vector.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We also need the mass matrix evaluated at the equilibrium and with the numerical values of the constants." ] }, { "cell_type": "code", "collapsed": false, "input": [ "M = mass_matrix.subs(equilibrium_dict)\n", "simplify(M)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "M = M.subs(parameter_dict)\n", "M = array(M.tolist(), dtype=float)\n", "print(M)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now we can use the NumPy matrix multiplication function `dot()` and the matrix inverse function `inv()` to compute the state $\\mathbf{A}$ and input $\\mathbf{B}$ matrices we need to create the controller." ] }, { "cell_type": "code", "collapsed": false, "input": [ "A = dot(inv(M), F_A)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "print(A)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 2, "metadata": {}, "source": [ "Exercise" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Create the input matrix, $\\mathbf{B}$ using `dot()` and `inv()`." ] }, { "cell_type": "code", "collapsed": false, "input": [], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "## Solution\n", "%load exercise_solutions/n09_control_input-matrix.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "print(B)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 1, "metadata": {}, "source": [ "Controller Design" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Before we try to create a controller, we need to check to see if it is actually [controllable](https://en.wikipedia.org/wiki/Controlability). You can import a function from the `utils` module included with these notebooks to check." ] }, { "cell_type": "code", "collapsed": false, "input": [ "from utils import controllable" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "controllable(A, B)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "So yes, the system is controllable. Now we will use [optimal control theory](https://en.wikipedia.org/wiki/Optimal_control) to create a controller that compute the necessary joint torques at each time step to balance the person.\n", "\n", "It turns out that if you setup a cost function in which you'd like to minimize the deviation from the desired state, 0, and minimize the effort in the joint torques, there is an elegant solution to find a matrix $\\mathbf{K}$ which can be used to compute the joint torques given the current state value.\n", "\n", "Given the cost function:\n", "\n", "$$J = \\frac{1}{2} \\int_0^\\infty\\left[\\mathbf{x}^T \\mathbf{Q} \\mathbf{x} + \\mathbf{u}^T \\mathbf{R} \\mathbf{u}\\right]dt$$\n", "\n", "There is a matrix $\\mathbf{K}$ that computes the inputs $\\mathbf{u}(t)$ given the states $\\mathbf{x}$:\n", "\n", "$$\\mathbf{u}(t) = -\\mathbf{K} \\mathbf{x}(t)$$\n", "\n", "where $\\mathbf{K}$ can be found by the solution to the algebraic [Riccati Equation](https://en.wikipedia.org/wiki/Riccati_equation) such that:\n", "\n", "$$\\mathbf{K} = \\mathbf{R}^{-1} \\mathbf{B}^T \\mathbf{S}$$\n", "\n", "and the Riccati equation is:\n", "\n", "$$\\textbf{0} = -\\textbf{S}\\textbf{A}-\\textbf{A}^{\\text{T}}\\textbf{S}+\\textbf{S}\\textbf{B}\\textbf{R}^{-1}\\textbf{B}^{\\text{T}}\\textbf{S}-\\textbf{Q}$$\n", "\n", "SciPy provides a function that can compute the solution, $\\mathbf{S}$, of the continous algebraic Riccati Equation." ] }, { "cell_type": "code", "collapsed": false, "input": [ "from scipy.linalg import solve_continuous_are" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The weighting matrices, $\\mathbf{Q}$ and $\\mathbf{R}$, allow you to control how much weight is applied to minimizing the error in the states and the effort in the inputs. A typical starting point for these matrices is to set them equal to the identity matrix, which can be created with the NumPy `eye()` function." ] }, { "cell_type": "code", "collapsed": false, "input": [ "Q = eye(6)\n", "Q" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "R = eye(3)\n", "R" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "$\\mathbf{K}$ can now be computed." ] }, { "cell_type": "code", "collapsed": false, "input": [ "S = solve_continuous_are(A, B, Q, R)\n", "K = dot(dot(inv(R), B.T), S)\n", "K" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Remember that the `right_hand_side` function we generated earlier can take a function for the specified values." ] }, { "cell_type": "code", "collapsed": false, "input": [ "help(right_hand_side)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 2, "metadata": {}, "source": [ "Exercise" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Create a function named `controller` that computes the specified inputs given the current state and time." ] }, { "cell_type": "code", "collapsed": false, "input": [ "def controller():\n", " \"\"\"Returns the output of the controller, i.e. the joint torques, given the current state.\"\"\"\n", " return" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "## Solution\n", "%load exercise_solutions/n09_control_controller-function.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now we can integrate the equations of motion again, but this time with the controller in place." ] }, { "cell_type": "code", "collapsed": false, "input": [ "args['specified'] = controller" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 2, "metadata": {}, "source": [ "Exercise" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Integrate the equations of motion and plot the coordinates and speeds as we did in the simulation notebook and see if the system behaves as expected." ] }, { "cell_type": "code", "collapsed": false, "input": [], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "## Solution\n", "%load exercise_solutions/n09_control_integrate-eom.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 1, "metadata": {}, "source": [ "Plot Trajectories" ] }, { "cell_type": "heading", "level": 2, "metadata": {}, "source": [ "Exercise" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Use the same plot functions as were used in the simulation notebook to inspect the state trajectories." ] }, { "cell_type": "code", "collapsed": false, "input": [ "# Plot the angles\n", "\n" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "## Solution\n", "%load exercise_solutions/n09_control_plot-angles.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "# Plot the angular velocities\n", "\n" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "## Solution\n", "%load exercise_solutions/n09_control_plot-velocities.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "What do these plots tell you about the controller? Does it work?" ] }, { "cell_type": "heading", "level": 1, "metadata": {}, "source": [ "Visualization" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Finally, regenerate the visualization to see the 3D animation of the system under the influence of the controller." ] }, { "cell_type": "code", "collapsed": false, "input": [ "scene.generate_visualization_json(coordinates + speeds, constants, y, numerical_constants)" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "scene.display()" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "heading", "level": 1, "metadata": {}, "source": [ "Exploratory Exercises" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now experiment with several of the parameters of our human model. Below is a taller and much heavier version of our model. See how the controller can make this person balance." ] }, { "cell_type": "code", "collapsed": false, "input": [ "numerical_constants = array([0.711, # lower_leg_length [m]\n", " 0.437, # lower_leg_com_length [m]\n", " 6.769, # lower_leg_mass [kg]\n", " 0.101, # lower_leg_inertia [kg*m^2]\n", " 0.524, # upper_leg_length [m]\n", " 0.243, # upper_leg_com_length\n", " 17.01, # upper_leg_mass [kg]\n", " 0.282, # upper_leg_inertia [kg*m^2]\n", " 10.355, # torso_com_length [m]\n", " 150.00, # torso_mass [kg]\n", " 1.485, # torso_inertia [kg*m^2]\n", " 9.81], # acceleration due to gravity [m/s^2]\n", " )" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "For convienence the early portions of the notebook repeated in this script. Just run the following cell after modifying the previous cell. " ] }, { "cell_type": "code", "collapsed": false, "input": [ "%run -i exercise_solutions/n09_control_snippit.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Create plots here to inspect the state trajectories" ] }, { "cell_type": "code", "collapsed": false, "input": [], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now experiment with the $\\mathbf{Q}$ and $\\mathbf{R}$ weighting matricies to see if you can come up with a better controller for this new model." ] }, { "cell_type": "code", "collapsed": false, "input": [ "Q = array([[ .1, 0., 0., 0., 0., 0.],\n", " [ 0., 1., 0., 0., 0., 0.],\n", " [ 0., 0., .1, 0., 0., 0.],\n", " [ 0., 0., 0., 1., 0., 0.],\n", " [ 0., 0., 0., 0., 1., 0.],\n", " [ 0., 0., 0., 0., 0., .1]])" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "R = array([[ 20., 0., 0.],\n", " [ 0., 1., 0.],\n", " [ 0., 0., 1.]])" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "code", "collapsed": false, "input": [ "%run -i exercise_solutions/n09_control_snippit.py" ], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Create plots here to inspect the state trajectories. " ] }, { "cell_type": "code", "collapsed": false, "input": [], "language": "python", "metadata": {}, "outputs": [] }, { "cell_type": "markdown", "metadata": {}, "source": [ "If you would like, regenerate the 3D visualization." ] }, { "cell_type": "code", "collapsed": false, "input": [ "scene.generate_visualization_json(coordinates + speeds, constants, y, numerical_constants)\n", "scene.display()" ], "language": "python", "metadata": {}, "outputs": [] } ], "metadata": {} } ] }