{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Credit"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This example is a (slightly modified) contribution by [Aykut Satici](https://github.com/symplectomorphism)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Setup"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We'll be using `RigidBodyDynamics` and `StaticArrays` for the simulation part."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "using RigidBodyDynamics\n",
    "using StaticArrays"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Model definition"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We're going to create a [four-bar linkage](https://en.wikipedia.org/wiki/Four-bar_linkage) that looks like this:\n",
    "<img src=\"fourbar.jpg\">\n",
    "\n",
    "We'll 'cut' the mechanism at joint 4: joints 1, 2, and 3 will be part of the spanning tree of the mechanism, but joint 4 will be a 'loop joint' (see e.g. Featherstone's 'Rigid Body Dynamics Algorithms'), for which the dynamics will be enforced using Lagrange multipliers."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First, we'll define some relevant constants:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# gravitational acceleration\n",
    "g = -9.81\n",
    "\n",
    "# link lengths\n",
    "l_0 = 1.10\n",
    "l_1 = 0.5\n",
    "l_2 = 1.20\n",
    "l_3 = 0.75\n",
    "\n",
    "# link masses\n",
    "m_1 = 0.5\n",
    "m_2 = 1.0\n",
    "m_3 = 0.75\n",
    "\n",
    "# link center of mass offsets from the preceding joint axes\n",
    "c_1 = 0.25\n",
    "c_2 = 0.60\n",
    "c_3 = 0.375\n",
    "\n",
    "# moments of inertia about the center of mass of each link\n",
    "I_1 = 0.333\n",
    "I_2 = 0.537\n",
    "I_3 = 0.4\n",
    "\n",
    "# scalar type\n",
    "T = Float64;\n",
    "\n",
    "# Rotation axis: negative y-axis\n",
    "axis = SVector(zero(T), -one(T), zero(T))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Construct the world rigid body and create a new mechanism."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Spanning tree:\n",
       "Vertex: world (root)\n",
       "No non-tree joints."
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "world = RigidBody{T}(\"world\")\n",
    "fourbar = Mechanism(world; gravity = SVector(0., 0., g))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we'll construct the spanning tree of the mechanism, consisting of bodies 1, 2, and 3 connected by joints 1, 2, and 3."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Spanning tree:\n",
       "Vertex: world (root)\n",
       "  Vertex: inertia1_centroidal, Edge: joint1\n",
       "    Vertex: inertia2_centroidal, Edge: joint2\n",
       "  Vertex: inertia3_centroidal, Edge: joint3\n",
       "No non-tree joints."
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# link1 and joint1\n",
    "joint1 = Joint(\"joint1\", Revolute(axis))\n",
    "inertia1 = SpatialInertia(CartesianFrame3D(\"inertia1_centroidal\"), I_1*axis*axis', zeros(SVector{3, T}), m_1)\n",
    "link1 = RigidBody(inertia1)\n",
    "beforeJoint1ToWorld = eye(Transform3D, frame_before(joint1), default_frame(world))\n",
    "c1_to_joint = Transform3D(inertia1.frame, frame_after(joint1), SVector(c_1, 0, 0))\n",
    "attach!(fourbar, world, joint1, beforeJoint1ToWorld, link1, c1_to_joint)\n",
    "\n",
    "# link2 and joint2\n",
    "joint2 = Joint(\"joint2\", Revolute(axis))\n",
    "inertia2 = SpatialInertia(CartesianFrame3D(\"inertia2_centroidal\"), I_2*axis*axis', zeros(SVector{3, T}), m_2)\n",
    "link2 = RigidBody(inertia2)\n",
    "beforeJoint2ToAfterJoint1 = Transform3D(frame_before(joint2), frame_after(joint1), SVector(l_1, 0., 0.))\n",
    "c2_to_joint = Transform3D(inertia2.frame, frame_after(joint2), SVector(c_2, 0, 0))\n",
    "attach!(fourbar, link1, joint2, beforeJoint2ToAfterJoint1, link2, c2_to_joint)\n",
    "\n",
    "# link3 and joint3\n",
    "joint3 = Joint(\"joint3\", Revolute(axis))\n",
    "inertia3 = SpatialInertia(CartesianFrame3D(\"inertia3_centroidal\"), I_3*axis*axis', zeros(SVector{3, T}), m_3)\n",
    "link3 = RigidBody(inertia3)\n",
    "beforeJoint3ToWorld = Transform3D(frame_before(joint3), default_frame(world), SVector(l_0, 0., 0.))\n",
    "c3_to_joint = Transform3D(inertia3.frame, frame_after(joint3), SVector(c_3, 0, 0))\n",
    "attach!(fourbar, world, joint3, beforeJoint3ToWorld, link3, c3_to_joint)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, we'll add joint 4 in almost the same way we did the other joints, with the following exceptions:\n",
    "1. both `link2` and `link3` are already part of the `Mechanism`, so the `attach!` function will figure out that `joint4` will be a loop joint.\n",
    "2. instead of using the default (identity) for the argument that specifies the transform from the successor of joint 4 (i.e., link 3) to the frame directly after joint 4, we'll specify a transform that incorporates the $l_3$ offset."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Spanning tree:\n",
       "Vertex: world (root)\n",
       "  Vertex: inertia1_centroidal, Edge: joint1\n",
       "    Vertex: inertia2_centroidal, Edge: joint2\n",
       "  Vertex: inertia3_centroidal, Edge: joint3\n",
       "Non-tree joints:\n",
       "joint4, predecessor: inertia2_centroidal, successor: inertia3_centroidal"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# joint between link2 and link3\n",
    "joint4 = Joint(\"joint4\", Revolute(axis))\n",
    "beforeJoint4ToJoint2 = Transform3D(frame_before(joint4), frame_after(joint2), SVector(l_2, 0., 0.))\n",
    "joint3ToAfterJoint4 = Transform3D(frame_after(joint3), frame_after(joint4), SVector(-l_3, 0., 0.))\n",
    "attach!(fourbar, link2, joint4, beforeJoint4ToJoint2, link3, joint3ToAfterJoint4)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note the additional non-tree joint in the printed `Mechanism` summary."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Simulation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As usual, we'll now construct a `MechanismState` and `DynamicsResult` for the four-bar `Mechanism`. We'll set some initial conditions for a simulation, which were solved for a priori using a nonlinear program (not shown here)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "state = MechanismState(T, fourbar)\n",
    "result = DynamicsResult{T, T}(fourbar);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "configuration(state, joint1)[:] = 1.6707963267948966 # θ\n",
    "configuration(state, joint2)[:] = -1.4591054166649482 # γ\n",
    "configuration(state, joint3)[:] = 1.5397303602625536 # ϕ\n",
    "\n",
    "velocity(state, joint1)[:] = 0.5\n",
    "velocity(state, joint2)[:] = -0.47295\n",
    "velocity(state, joint3)[:] = 0.341\n",
    "\n",
    "# Invalidate the cache variables\n",
    "setdirty!(state)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Next, we'll do a 3-second simulation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "ts, qs, vs = simulate(state, 3., Δt = 1e-2);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Visualization"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For visualization, we'll use [`RigidBodyTreeInspector`](https://github.com/rdeits/RigidBodyTreeInspector.jl).\n",
    "\n",
    "(Note: the `#NBSKIP` comments are to skip these cells during `Pkg.test()`)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div id=\"interact-js-shim\">\n",
       "    <script charset=\"utf-8\">\n",
       "(function (IPython, $, _, MathJax) {\n",
       "    $.event.special.destroyed = {\n",
       "\tremove: function(o) {\n",
       "\t    if (o.handler) {\n",
       "\t\to.handler.apply(this, arguments)\n",
       "\t    }\n",
       "\t}\n",
       "    }\n",
       "\n",
       "    var OutputArea = IPython.version >= \"4.0.0\" ? require(\"notebook/js/outputarea\").OutputArea : IPython.OutputArea;\n",
       "\n",
       "    var redrawValue = function (container, type, val) {\n",
       "\tvar selector = $(\"<div/>\");\n",
       "\tvar oa = new OutputArea(_.extend(selector, {\n",
       "\t    selector: selector,\n",
       "\t    prompt_area: true,\n",
       "\t    events: IPython.events,\n",
       "\t    keyboard_manager: IPython.keyboard_manager\n",
       "\t})); // Hack to work with IPython 2.1.0\n",
       "\n",
       "\tswitch (type) {\n",
       "\tcase \"image/png\":\n",
       "            var _src = 'data:' + type + ';base64,' + val;\n",
       "\t    $(container).find(\"img\").attr('src', _src);\n",
       "\t    break;\n",
       "\tcase \"text/latex\":\n",
       "\t\tif (MathJax){\n",
       "\t\t\tvar math = MathJax.Hub.getAllJax(container)[0];\n",
       "\t\t\tMathJax.Hub.Queue([\"Text\", math, val.replace(/^\\${1,2}|\\${1,2}$/g, '')]);\n",
       "\t\t\tbreak;\n",
       "\t\t}\n",
       "\tdefault:\n",
       "\t    var toinsert = OutputArea.append_map[type].apply(\n",
       "\t\toa, [val, {}, selector]\n",
       "\t    );\n",
       "\t    $(container).empty().append(toinsert.contents());\n",
       "\t    selector.remove();\n",
       "\t}\n",
       "    }\n",
       "\n",
       "\n",
       "    $(document).ready(function() {\n",
       "\tfunction initComm(evt, data) {\n",
       "\t    var comm_manager = data.kernel.comm_manager;\n",
       "        //_.extend(comm_manager.targets, require(\"widgets/js/widget\"))\n",
       "\t    comm_manager.register_target(\"Signal\", function (comm) {\n",
       "            comm.on_msg(function (msg) {\n",
       "                var val = msg.content.data.value;\n",
       "                $(\".signal-\" + comm.comm_id).each(function() {\n",
       "                var type = $(this).data(\"type\");\n",
       "                if (typeof(val[type]) !== \"undefined\" && val[type] !== null) {\n",
       "                    redrawValue(this, type, val[type], type);\n",
       "                }\n",
       "                });\n",
       "                delete val;\n",
       "                delete msg.content.data.value;\n",
       "            });\n",
       "\t    });\n",
       "\n",
       "\t    // coordingate with Comm and redraw Signals\n",
       "\t    // XXX: Test using Reactive here to improve performance\n",
       "\t    $([IPython.events]).on(\n",
       "\t\t'output_appended.OutputArea', function (event, type, value, md, toinsert) {\n",
       "\t\t    if (md && md.reactive) {\n",
       "                // console.log(md.comm_id);\n",
       "                toinsert.addClass(\"signal-\" + md.comm_id);\n",
       "                toinsert.data(\"type\", type);\n",
       "                // Signal back indicating the mimetype required\n",
       "                var comm_manager = IPython.notebook.kernel.comm_manager;\n",
       "                var comm = comm_manager.comms[md.comm_id];\n",
       "                comm.then(function (c) {\n",
       "                    c.send({action: \"subscribe_mime\",\n",
       "                       mime: type});\n",
       "                    toinsert.bind(\"destroyed\", function() {\n",
       "                        c.send({action: \"unsubscribe_mime\",\n",
       "                               mime: type});\n",
       "                    });\n",
       "                })\n",
       "\t\t    }\n",
       "\t    });\n",
       "\t}\n",
       "\n",
       "\ttry {\n",
       "\t    // try to initialize right away. otherwise, wait on the status_started event.\n",
       "\t    initComm(undefined, IPython.notebook);\n",
       "\t} catch (e) {\n",
       "\t    $([IPython.events]).on('kernel_created.Kernel kernel_created.Session', initComm);\n",
       "\t}\n",
       "    });\n",
       "})(IPython, jQuery, _, MathJax);\n",
       "</script>\n",
       "    <script>\n",
       "        window.interactLoadedFlag = true\n",
       "       $(\"#interact-js-shim\").bind(\"destroyed\", function () {\n",
       "           if (window.interactLoadedFlag) {\n",
       "               console.warn(\"JavaScript required by Interact will be removed if you remove this cell or run using Interact more than once.\")\n",
       "           }\n",
       "       })\n",
       "       $([IPython.events]).on(\"kernel_starting.Kernel kernel_restarting.Kernel\", function () { window.interactLoadedFlag = false })\n",
       "   </script>\n",
       "</div>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "\u001b[1m\u001b[34mINFO: Checking out RigidBodyTreeInspector master...\n",
      "\u001b[0m\u001b[1m\u001b[34mINFO: Pulling RigidBodyTreeInspector latest master...\n",
      "\u001b[0m\u001b[1m\u001b[34mINFO: No packages to install, update or remove\n",
      "\u001b[0m"
     ]
    }
   ],
   "source": [
    "#NBSKIP\n",
    "using RigidBodyTreeInspector\n",
    "Pkg.checkout(\"RigidBodyTreeInspector\") # use the master branch for now"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Open the viewer application if it isn't open already:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "DrakeVisualizer.any_open_windows() || (DrakeVisualizer.new_window(); sleep(1));"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Load the mechanism's geometry into the visualizer:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "vis = DrakeVisualizer.Visualizer()[:robot1];\n",
    "RigidBodyTreeInspector.setgeometry!(vis, RigidBodyTreeInspector.create_geometry(fourbar));"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And animate:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#NBSKIP\n",
    "RigidBodyTreeInspector.animate(vis, fourbar, ts, qs; realtimerate = 1.);"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.5.1",
   "language": "julia",
   "name": "julia-0.5"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.5.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}