{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Introduction"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now have defined all of the necessary components to derive the equations of motion of the system using Kane's Method:\n",
    "\n",
    "- Linear velocity of each mass center.\n",
    "- Angular velocity of each rigid body.\n",
    "- All contributing (external) forces.\n",
    "\n",
    "We will use the `KanesMethod` class which provides an automated computation of the first order ordinary differential equations given the above quantities. (Any method can be used, such as Newton-Euler or Hamilton's method, but there are only automated methods for Kane's equations and Lagrangian dynamics at the moment)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Setup"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Import the solutions from the previous notebooks:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from __future__ import print_function, division\n",
    "from solution.kinetics import *"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will need the `KanesMethod` class and `trigsimp` to give a reasonably compact result."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from sympy import trigsimp\n",
    "from sympy.physics.mechanics import KanesMethod"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Enable nice math printing:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from sympy.physics.vector import init_vprinting\n",
    "init_vprinting(use_latex='mathjax', pretty_print=False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "KanesMethod?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Equations of Motion"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the bare minimum for unconstrained systems, the `KanesMethod` class needs to know the generalized coordinates, the generalized speeds, the kinematical differential equations, the loads, the bodies, and a Newtonian reference frame. First, make a list of the generalized coordinates, i.e. the three joint angles:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "coordinates = [theta1, theta2, theta3]\n",
    "coordinates"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now list the generalized speeds, i.e. the joint rates:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "speeds = [omega1, omega2, omega3]\n",
    "speeds"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `KanesMethod` object can now be created with the inertial reference frame, coordinates, speeds, and kinematical differential equations:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "kinematical_differential_equations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "kane = KanesMethod(inertial_frame, coordinates, speeds, kinematical_differential_equations)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now list the six loads we defined and the three bodies:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "loads = [lower_leg_grav_force,\n",
    "         upper_leg_grav_force,\n",
    "         torso_grav_force, \n",
    "         lower_leg_torque,\n",
    "         upper_leg_torque,\n",
    "         torso_torque]\n",
    "loads"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "bodies = [lower_leg, upper_leg, torso]\n",
    "bodies"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The equations of motion can now be computed using the `kanes_equations` method, which takes the list of loads and bodies. It returns the equations of motion (i.e. first order ordinary differential equations) in Kane's form:\n",
    "\n",
    "$$ F_r + F_r^* = 0$$\n",
    "\n",
    "which is essentially equivalent to the classic Newton_Euler form:\n",
    "\n",
    "$$ \\mathbf{F} = \\mathbf{m}\\mathbf{a} $$\n",
    "\n",
    "$$ \\mathbf{T} = \\mathbf{I} \\mathbf{\\alpha} $$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "fr, frstar = kane.kanes_equations(bodies, loads)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "trigsimp(fr + frstar)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Keep in mind that out utlimate goal is to have the equations of motion in first order form:\n",
    "\n",
    "$$ \\dot{\\mathbf{x}} = \\mathbf{g}(\\mathbf{x}, t) $$\n",
    "\n",
    "The equations of motion are linear in terms of the derivatives of the generalized speeds and the `KanesMethod` class automatically puts the equations in a more useful form for the next step of numerical simulation:\n",
    "\n",
    "$$ \\mathbf{M}(\\mathbf{x}, t)\\dot{\\mathbf{x}} = \\mathbf{f}(\\mathbf{x}, t) $$\n",
    "\n",
    "Note that\n",
    "\n",
    "$$ \\mathbf{g} = \\mathbf{M}^{-1}(\\mathbf{x}, t) \\mathbf{f}(\\mathbf{x}, t) $$\n",
    "\n",
    "and that $\\mathbf{g}$ can be computed analytically but for non-toy problems, it is best to do this numerically. So we will simply generate the $\\mathbf{M}$ and $\\mathbf{f}$ matrices for later use.\n",
    "\n",
    "The mass matrix, $\\mathbf{M}$, can be accessed with the `mass_matrix` method (use `mass_matrix_full` to include the kinematical differential equations too. We can use `trigsimp` again to make this relatively compact: "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "mass_matrix = trigsimp(kane.mass_matrix_full)\n",
    "mass_matrix"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The right hand side, $\\mathbf{f}$, is a vector function of all the non-inertial forces (gyroscopic, external, coriolis, etc):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "forcing_vector = trigsimp(kane.forcing_full)\n",
    "forcing_vector"
   ]
  }
 ],
 "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
}