{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Introduction"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now that we have the symbolic equations of motion we need to transform them into Python functions that can be evaluated for use in numerical integration. [Numerical integration](http://en.wikipedia.org/wiki/Numerical_methods_for_ordinary_differential_equations) is required to solve the ordinary differential initial value problem and allow us to see how the states change through time."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Setup"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Load the solutions from the previous notebooks:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from solution.equations_of_motion import *"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To setup the numerical values and integrate the equations of motion we will need some functions from NumPy for numerical arrays:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from numpy import deg2rad, rad2deg, array, zeros, linspace"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will need a ODE numerical integration routine from SciPy:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from scipy.integrate import odeint"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can use PyDy's ODE function generator to transform the symbolic equations into numerical functions:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from pydy.codegen.ode_function_generators import generate_ode_function"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once again, let's display the symbolics nicely."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from sympy.physics.vector import init_vprinting, vlatex"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "init_vprinting(use_latex='mathjax', pretty_print=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Once we get a solution for how the state changes over time, it is nice to visualize it. The simplest way to do this is to plot the trajectories versus time. We can use the matplotlib library to do this. First enable inline plotting:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Import a few functions for plotting:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from matplotlib.pyplot import plot, legend, xlabel, ylabel, rcParams"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And set the default figure size to be larger:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "rcParams['figure.figsize'] = (14.0, 6.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Variables"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first step is to gather all of the variables in the equations of motion into lists. We will need the constants, coordinates, speeds, and the specified inputs."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Constants"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There are twelve constants in the equations. Put them into a list."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "constants = [lower_leg_length,\n",
    "             lower_leg_com_length,\n",
    "             lower_leg_mass,\n",
    "             lower_leg_inertia,\n",
    "             upper_leg_length,\n",
    "             upper_leg_com_length,\n",
    "             upper_leg_mass,\n",
    "             upper_leg_inertia,\n",
    "             torso_com_length,\n",
    "             torso_mass,            \n",
    "             torso_inertia,\n",
    "             g]\n",
    "constants\n",
    "             "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Time Varying"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The coordinates and speeds make up the states and there are three time varying specified inputs to the system, the joint torques."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "coordinates = [theta1, theta2, theta3]\n",
    "coordinates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "speeds = [omega1, omega2, omega3]\n",
    "speeds"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercise"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Make a list called `specified` that contains the three torque magnitude variables: $T_a$, $T_k$, and $T_h$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%load exercise_solutions/n07_simulation_torque-magnitude.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Generate the Numerical ODE Function"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Ordinary differential equation integrators, like `scipy.integrate.odeint`, require a function that numerically evaluates the right hand side of the coupled first order ordinary differential equations. We have the symbolic form of the mass matrix and the forcing vector available. The `generate_ode_function` function generates a function from the symbolic expressions that fits the form needed for `odeint`.\n",
    "\n",
    "`odeint` is an ODE integrator based on the `lsoda` routine from ODEPACK that works well for both non-stiff and stiff ODEs. Notice that it requres the right hand side function, the initial conditions of the state, a time vector. We will also pass in extra arguments, `args`, of the right hand side function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "help(odeint)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To create the function, simply pass in $\\mathbf{M}$, $\\mathbf{f}$, and the lists of variables in the system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "right_hand_side = generate_ode_function(forcing_vector, coordinates,\n",
    "                                        speeds, constants,\n",
    "                                        mass_matrix=mass_matrix,\n",
    "                                        specifieds=specified)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We see that the result is a function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "type(right_hand_side)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And the doc string gives information on the type of the arguments needed to evaluate it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "help(right_hand_side)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Set the Initial Conditions, Parameter Values, and Time Array"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will set the intial values of the speeds to be zero and the coordinates to be offset from vertical at 2 degrees. First make an array of zeros:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "x0 = zeros(6)\n",
    "x0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And then set the first three values, $\\theta_{1,2,3}$, to 2 degrees:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "x0[:3] = deg2rad(2.0)\n",
    "x0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The right hand side function requires numerical values of all the constants to be passed in and values for the specified joint torques. Here we will use typical values from body segment parameter measurements which were generated from the [Yeadon](http://yeadon.readthedocs.org/en/latest/) Python package (`male1.txt`). Make sure the units are all consistent!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "numerical_constants = array([0.611,  # lower_leg_length [m]\n",
    "                             0.387,  # lower_leg_com_length [m]\n",
    "                             6.769,  # lower_leg_mass [kg]\n",
    "                             0.101,  # lower_leg_inertia [kg*m^2]\n",
    "                             0.424,  # upper_leg_length [m]\n",
    "                             0.193,  # upper_leg_com_length\n",
    "                             17.01,  # upper_leg_mass [kg]\n",
    "                             0.282,  # upper_leg_inertia [kg*m^2]\n",
    "                             0.305,  # torso_com_length [m]\n",
    "                             32.44,  # torso_mass [kg]\n",
    "                             1.485,  # torso_inertia [kg*m^2]\n",
    "                             9.81],  # acceleration due to gravity [m/s^2]\n",
    "                            ) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercise"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For this first simulation we will set the three joint torques equal to zero for the duration of the simulation. Created a Python variale `numerical_specified` which is a NumPy array of length three and each entry is equal to zero."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can use the `linspace` function to generate a time vector over 10 secs such that `odeint` returns results at 60 Hz."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%load exercise_solutions/n07_simulation_sim-setup.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Integrate the Equations of Motion"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The right hand side function can now be evaluated numerically given a current value of the states, a value for time, and the numerical values for all the constants and specified values in the equations of motion:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "right_hand_side(x0, 0.0, numerical_specified, numerical_constants)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can solve the initial value problem and simulate the motion. As shown above, `odeint` requires the function to integrate `right_hand_side`, the initial conditions `x0`, the time vector `t`, and the extra arguments `args`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "y = odeint(right_hand_side, x0, t, args=(numerical_specified, numerical_constants))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `y` variable now contains a 2D array that gives the trajectories of the states as a function of time."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "y.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Plot the results"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can plot the first 3 columns of `y` versus `t` so see how the three angles change throughout time."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "plot(t, rad2deg(y[:, :3]))\n",
    "xlabel('Time [s]')\n",
    "ylabel('Angle [deg]')\n",
    "legend([\"${}$\".format(vlatex(c)) for c in coordinates])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What does this graph tell us? How does the system behave?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercise"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now as an exercise, plot the generalized speeds (i.e. the last three states)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%load exercise_solutions/n07_simulation_plot-speeds.py"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}