{
 "cells": [
  {
   "cell_type": "markdown",
   "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": "markdown",
   "metadata": {},
   "source": [
    "# Setup"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Display the problem diagram for reference:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from __future__ import print_function, division\n",
    "from IPython.display import Image\n",
    "Image('figures/human_balance_diagram.png')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from solution.simulation import *"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We import the shapes we are going to use in our visualization."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from pydy.viz.shapes import Cylinder, Sphere\n",
    "import pydy.viz\n",
    "pydy.viz.shapes.__all__"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Import other methods from the package!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from pydy.viz.visualization_frame import VisualizationFrame\n",
    "from pydy.viz.scene import Scene"
   ]
  },
  {
   "cell_type": "markdown",
   "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",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Sphere?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "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)"
   ]
  },
  {
   "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",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "VisualizationFrame?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "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)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here create a point for a \"Head\" and teh `VisualizationFrame`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "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)"
   ]
  },
  {
   "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",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "lower_leg_center = Point('l_c')\n",
    "upper_leg_center = Point('u_c')\n",
    "torso_center = Point('t_c')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "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)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It will be helpful to have easy access to the constants to specify the cylinder lengths."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "constants_dict = dict(zip(constants, numerical_constants))\n",
    "constants_dict"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now create some cylinders to attach to the three rigid bodies."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Cylinder?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "lower_leg_shape = Cylinder(radius=0.08, 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)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "upper_leg_shape = Cylinder(radius=0.08, 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)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "torso_shape = Cylinder(radius=0.08, length=2 * constants_dict[torso_com_length], color='red')\n",
    "\n",
    "torso_viz_frame = VisualizationFrame('Torso', torso_frame, torso_center, torso_shape)"
   ]
  },
  {
   "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",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Scene?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "scene = Scene(inertial_frame, ankle)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we append the frames we wish to visualize in the scene object, as a list."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "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]"
   ]
  },
  {
   "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."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "scene.states_symbols = coordinates + speeds\n",
    "scene.constants = constants_dict\n",
    "scene.states_trajectories = y"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we can display the visualization of the free motion of the system in the cell with the following method:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "scene.display_ipython()"
   ]
  },
  {
   "cell_type": "markdown",
   "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": "markdown",
   "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": {
  "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.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}