{
 "metadata": {
  "name": ""
 },
 "nbformat": 3,
 "nbformat_minor": 0,
 "worksheets": [
  {
   "cells": [
    {
     "cell_type": "heading",
     "level": 1,
     "metadata": {},
     "source": [
      "Introduction"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "In this notebook you will learn how to make a simple 3D visualization of the motion of the human using [pydy-viz](https://github.com/pydy/pydy-viz)."
     ]
    },
    {
     "cell_type": "heading",
     "level": 1,
     "metadata": {},
     "source": [
      "Setup"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Display the problem diagram for reference:"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "from IPython.display import Image\n",
      "Image('figures/human_balance_diagram.png')"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "from solution.simulation import *"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "We import the shapes we are going to use in our visualization."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "from pydy_viz.shapes import Cylinder, Sphere"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Import other methods from the package!"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "from pydy_viz.visualization_frame import VisualizationFrame\n",
      "from pydy_viz.scene import Scene"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "heading",
     "level": 1,
     "metadata": {},
     "source": [
      "Visualization"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "First we need to create shapes to associate with rigid body. We will start by creating a sphere to represent each joint."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "Sphere?"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "ankle_shape = Sphere(color='black', radius=0.1)\n",
      "knee_shape = Sphere(color='black', radius=0.1)\n",
      "hip_shape = Sphere(color='black', radius=0.1)\n",
      "head_shape = Sphere(color='black', radius=0.125)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Now create a visualization frame for each of the shapes. A `VisualizationFrame` object essentially attaches a shape to a reference frame and a point. Since it doesn't matter if the spheres rotate, we can simply attach them to the inertial reference frame."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "VisualizationFrame?"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "ankle_viz_frame = VisualizationFrame(inertial_frame, ankle, ankle_shape)\n",
      "knee_viz_frame = VisualizationFrame(inertial_frame, knee, knee_shape)\n",
      "hip_viz_frame = VisualizationFrame(inertial_frame, hip, hip_shape)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Here create a point for a \"Head\" and teh `VisualizationFrame`."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "head = Point('N')  # N for Noggin\n",
      "head.set_pos(hip, 2 * torso_com_length * torso_frame.y)\n",
      "head_viz_frame = VisualizationFrame(inertial_frame, head, head_shape)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "We will also need some points for the geometric center of the lower leg, upper leg, and torso. If we attache a `Cylinder` shape to those bodies, the `Cylinder` is defined with it's long axis along the Y direction of the attached reference frame and the geometric center is the local origin."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "lower_leg_center = Point('l_c')\n",
      "upper_leg_center = Point('u_c')\n",
      "torso_center = Point('t_c')"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "lower_leg_center.set_pos(ankle, lower_leg_length / 2 * lower_leg_frame.y)\n",
      "upper_leg_center.set_pos(knee, upper_leg_length / 2 * upper_leg_frame.y)\n",
      "torso_center.set_pos(hip, torso_com_length * torso_frame.y)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "It will be helpful to have easy access to the constants to specify the cylinder lengths."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "constants_dict = dict(zip(constants, numerical_constants))\n",
      "constants_dict"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Now create some cylinders to attach to the three rigid bodies."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "Cylinder?"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "lower_leg_shape = Cylinder('Lower Leg Cylinder', radius=0.08,\n",
      "                      length=constants_dict[lower_leg_length], color='blue')\n",
      "\n",
      "lower_leg_viz_frame = VisualizationFrame('Lower Leg', lower_leg_frame, lower_leg_center, lower_leg_shape)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "upper_leg_shape = Cylinder('Upper Leg Cylinder', radius=0.08,\n",
      "                      length=constants_dict[upper_leg_length], color='green')\n",
      "\n",
      "upper_leg_viz_frame = VisualizationFrame('Upper Leg', upper_leg_frame, upper_leg_center, upper_leg_shape)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "torso_shape = Cylinder('Torso Cylinder', radius=0.08,\n",
      "                      length=2 * constants_dict[torso_com_length], color='red')\n",
      "\n",
      "torso_viz_frame = VisualizationFrame('Torso', torso_frame, torso_center, torso_shape)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Now we create a scene whose base frame is the system's inertial frame and whose origin is the ankle point."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "Scene?"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "scene = Scene(inertial_frame, ankle)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Now we append the frames we wish to visualize in the scene object, as a list."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "scene.visualization_frames = [ankle_viz_frame,\n",
      "                              knee_viz_frame,\n",
      "                              hip_viz_frame,\n",
      "                              head_viz_frame,\n",
      "                              lower_leg_viz_frame,\n",
      "                              upper_leg_viz_frame,\n",
      "                              torso_viz_frame]"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "We provide the scene with the list of symbolic states, the list of constants, as well as the numerical values of each.\n",
      "The following method takes all the states and constants and generates a json object compatible with the pydy-viz frontend (PyDy-Visualizer)."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "scene.generate_visualization_json(coordinates + speeds, constants, y, numerical_constants)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "The following method will run a webserver at http://localhost:8000 (If port 8000 is taken, we try 8001, 8002, and so on until we find a free port). It also opens the visualization in the new tab. You can close the webserver by hitting the \"Shutdown Server\" button in the PyDy-Visualizer frontend."
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "scene.display()"
     ],
     "language": "python",
     "metadata": {},
     "outputs": []
    },
    {
     "cell_type": "heading",
     "level": 2,
     "metadata": {},
     "source": [
      "Exercise"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Edit the notebook above and try to add a nose to the \"head\" sphere in the visualization so you can tell which direction the person is facing. The nose should point in the $x$ direction. Maybe try the `Cone` shape in `pydy_viz.shapes`. You may have to create new reference frames or points to get the the shape oriented and positioned correctly."
     ]
    },
    {
     "cell_type": "heading",
     "level": 2,
     "metadata": {},
     "source": [
      "Exercise"
     ]
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Can you make the visualization look more human like? Maybe cylinders for each leg? Cylinders for arms? See what shapes are available in `pydy_viz.shapes`. You may have to create new reference frames or points to get the the shapes oriented and positioned correctly. Edit the notebook above to see if you can improve the visualization."
     ]
    }
   ],
   "metadata": {}
  }
 ]
}