{
 "metadata": {
  "name": "",
  "signature": "sha256:87658ca1f42520e7c7550a5ab77adea0ada554e982e5db05548e4d88a773e411"
 },
 "nbformat": 3,
 "nbformat_minor": 0,
 "worksheets": [
  {
   "cells": [
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "%matplotlib inline\n",
      "from pylab import *\n",
      "from optoy import *"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 1
    },
    {
     "cell_type": "markdown",
     "metadata": {},
     "source": [
      "Equations of a ball in freefall:\n",
      "\n",
      "$\\dot{x}=v_x$\n",
      "\n",
      "$\\dot{y}=v_y$\n",
      "\n",
      "$\\dot{v_x}=0$\n",
      "\n",
      "$\\dot{v_y}=-g$"
     ]
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "gravity = par()\n",
      "\n",
      "states = struct([\"x\",\"y\",\"vx\",\"vy\"])\n",
      "\n",
      "# Ode model for a ball\n",
      "#      state   params constant over stage\n",
      "def f( x,      dt,g   ):\n",
      "    return dt*vertcat([\n",
      "        x[2],\n",
      "        x[3],\n",
      "        0,\n",
      "        -g\n",
      "    ])\n",
      "\n",
      "simulator = OdeSimulator(f,T=1)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 2
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "stage1_x0 = var(shape=4) # Freefall\n",
      "stage1_t  = var()\n",
      "\n",
      "stage2_x0 = var(shape=4) # Bounce\n",
      "\n",
      "stage3_x0 = var(shape=4) # Freefall\n",
      "stage3_t  = var()\n",
      "\n",
      "stage4_x  = var(shape=4) # End\n",
      "\n",
      "# For easier indexing using labels\n",
      "state1 = states(stage1_x0)\n",
      "state2 = states(stage2_x0)\n",
      "state3 = states(stage3_x0)\n",
      "state4 = states(stage4_x)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 3
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# Expression obtained by integrating the ode over symbolic input\n",
      "xf1 = simulator(stage1_x0, stage1_t,gravity )\n",
      "xf2 = stage2_x0*vertcat([1,1,1,-0.5])\n",
      "xf3 = simulator(stage3_x0, stage3_t,gravity )"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 4
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# Objective\n",
      "f = -state4[\"x\"]\n",
      "\n",
      "g = [\n",
      "        state1[\"vx\"]**2 + state1[\"vy\"]**2 == 1, #normalized speed\n",
      "        state2[\"y\"] == 0, # Ground contact\n",
      "        stage2_x0 == xf1, # Transition\n",
      "        stage3_x0 == xf2, # Transition\n",
      "        state4[\"y\"] ==0, # Ground contact\n",
      "        stage4_x == xf3, # Transition   \n",
      "        state1[\"x\"] == 0,\n",
      "        state1[\"y\"] == 0,\n",
      "        stage1_t >= 0,\n",
      "        stage3_t >= 0\n",
      "]\n",
      "\n",
      "stage1_x0.init = [0,0,1,1]\n",
      "stage3_x0.init = [0,0,1,1]\n",
      "\n",
      "stage1_t.init = stage3_t.init = 2\n",
      "\n",
      "gravity.value = 1 # [m/s^2]\n",
      "\n",
      "solver = minimize(f,g,verbose=True)\n",
      "\n",
      "print value(stage4_x)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "******************************************************************************\n",
        "This program contains Ipopt, a library for large-scale nonlinear optimization.\n",
        " Ipopt is released as open source code under the Eclipse Public License (EPL).\n",
        "         For more information visit http://projects.coin-or.org/Ipopt\n",
        "******************************************************************************\n",
        "\n",
        "This is Ipopt version 3.11.9, running with linear solver mumps.\n",
        "NOTE: Other linear solvers might be more efficient (see Ipopt documentation).\n",
        "\n",
        "Number of nonzeros in equality constraint Jacobian...:       42\n",
        "Number of nonzeros in inequality constraint Jacobian.:        2\n",
        "Number of nonzeros in Lagrangian Hessian.............:        8\n",
        "\n",
        "Total number of variables............................:       18\n",
        "                     variables with only lower bounds:        0\n",
        "                variables with lower and upper bounds:        0\n",
        "                     variables with only upper bounds:        0\n",
        "Total number of equality constraints.................:       17\n",
        "Total number of inequality constraints...............:        2\n",
        "        inequality constraints with only lower bounds:        0\n",
        "   inequality constraints with lower and upper bounds:        0\n",
        "        inequality constraints with only upper bounds:        2\n",
        "\n",
        "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "   0 -0.0000000e+00 2.00e+00 2.29e-01  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0\n",
        "   1 -1.1769348e+00 3.80e-01 6.19e-01  -1.0 1.18e+00    -  1.00e+00 1.00e+00h  1\n",
        "   2 -1.5143703e+00 8.81e-03 5.51e-02  -1.7 3.37e-01    -  1.00e+00 1.00e+00h  1"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "   3 -1.5020642e+00 7.06e-04 3.42e-03  -2.5 4.24e-02    -  1.00e+00 1.00e+00h  1\n",
        "   4 -1.5000133e+00 4.57e-06 2.53e-05  -3.8 3.47e-03    -  1.00e+00 1.00e+00h  1"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "   5 -1.5000000e+00 3.18e-09 1.86e-08  -5.7 8.29e-05    -  1.00e+00 1.00e+00h  1\n",
        "   6 -1.5000000e+00 3.85e-13 2.18e-12  -8.6 8.79e-07    -  1.00e+00 1.00e+00h  1"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "\n",
        "Number of Iterations....: 6\n",
        "\n",
        "                                   (scaled)                 (unscaled)\n",
        "Objective...............:  -1.5000000000011544e+00   -1.5000000000011544e+00\n",
        "Dual infeasibility......:   2.1780355297096321e-12    2.1780355297096321e-12\n",
        "Constraint violation....:   3.8458125573015423e-13    3.8458125573015423e-13\n",
        "Complementarity.........:   2.5070550732432494e-09    2.5070550732432494e-09\n",
        "Overall NLP error.......:   2.5070550732432494e-09    2.5070550732432494e-09\n",
        "\n",
        "\n",
        "Number of objective function evaluations             = 7\n",
        "Number of objective gradient evaluations             = 7\n",
        "Number of equality constraint evaluations            = 7\n",
        "Number of inequality constraint evaluations          = 7\n",
        "Number of equality constraint Jacobian evaluations   = 7\n",
        "Number of inequality constraint Jacobian evaluations = 7\n",
        "Number of Lagrangian Hessian evaluations             = 6\n",
        "Total CPU secs in IPOPT (w/o function evaluations)   =      0.020\n",
        "Total CPU secs in NLP function evaluations           =      0.284\n",
        "\n",
        "EXIT: Optimal Solution Found.\n",
        "                   user           real      num           mean             mean\n",
        "                   time           time     evals       user time        real time\n",
        "       eval_f     0.006 [s]      0.006 [s]     7       0.92 [ms]        0.92 [ms]\n",
        "  eval_grad_f     0.044 [s]      0.044 [s]     8       5.47 [ms]        5.47 [ms]\n",
        "       eval_g     0.007 [s]      0.007 [s]     7       0.99 [ms]        0.99 [ms]\n",
        "   eval_jac_g     0.062 [s]      0.061 [s]     9       6.84 [ms]        6.81 [ms]\n",
        "       eval_h     0.182 [s]      0.182 [s]     7      26.05 [ms]       26.02 [ms]\n",
        " all previous     0.301 [s]      0.301 [s]\n",
        "        ipopt     0.009 [s]      0.008 [s]\n",
        "    main loop     0.310 [s]      0.308 [s]\n",
        "[1.5, 0, 0.707107, -0.353553]\n"
       ]
      }
     ],
     "prompt_number": 5
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "gravity.value = 2 # [m/s^2]\n",
      "solver.update()\n",
      "\n",
      "print value(stage4_x)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "This is Ipopt version 3.11.9, running with linear solver mumps.\n",
        "NOTE: Other linear solvers might be more efficient (see Ipopt documentation).\n",
        "\n",
        "Number of nonzeros in equality constraint Jacobian...:       42\n",
        "Number of nonzeros in inequality constraint Jacobian.:        2\n",
        "Number of nonzeros in Lagrangian Hessian.............:        8\n",
        "\n",
        "Total number of variables............................:       18\n",
        "                     variables with only lower bounds:        0\n",
        "                variables with lower and upper bounds:        0\n",
        "                     variables with only upper bounds:        0\n",
        "Total number of equality constraints.................:       17\n",
        "Total number of inequality constraints...............:        2\n",
        "        inequality constraints with only lower bounds:        0\n",
        "   inequality constraints with lower and upper bounds:        0\n",
        "        inequality constraints with only upper bounds:        2\n",
        "\n",
        "iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls\n"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "   0 -0.0000000e+00 3.00e+00 3.94e-01  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0\n",
        "   1 -3.6826555e+00 1.16e+00 1.51e+00  -1.0 3.68e+00    -  1.00e+00 1.00e+00h  1\n",
        "   2 -9.0403331e-01 3.12e-01 6.58e-01  -1.0 2.78e+00    -  1.00e+00 1.00e+00h  1"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "   3 -9.3935714e-01 2.79e-01 1.95e-01  -1.7 4.26e-01    -  7.58e-01 1.00e+00h  1\n",
        "   4 -7.5419675e-01 1.68e-02 3.53e-02  -1.7 1.85e-01    -  1.00e+00 1.00e+00h  1"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "   5 -7.5029209e-01 2.45e-04 5.52e-04  -2.5 1.70e-02    -  1.00e+00 1.00e+00h  1\n",
        "   6 -7.5000687e-01 4.45e-06 1.57e-05  -3.8 1.57e-03    -  1.00e+00 1.00e+00h  1"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "   7 -7.5000002e-01 1.17e-08 4.11e-08  -5.7 7.81e-05    -  1.00e+00 1.00e+00h  1\n",
        "   8 -7.5000000e-01 1.57e-12 5.52e-12  -8.6 8.89e-07    -  1.00e+00 1.00e+00h  1"
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "\n",
        "\n",
        "Number of Iterations....: 8\n",
        "\n",
        "                                   (scaled)                 (unscaled)\n",
        "Objective...............:  -7.5000000000234812e-01   -7.5000000000234812e-01\n",
        "Dual infeasibility......:   5.5214418522120758e-12    5.5214418522120758e-12\n",
        "Constraint violation....:   1.5651924201165457e-12    1.5651924201165457e-12\n",
        "Complementarity.........:   2.5082416129668846e-09    2.5082416129668846e-09\n",
        "Overall NLP error.......:   2.5082416129668846e-09    2.5082416129668846e-09\n",
        "\n",
        "\n",
        "Number of objective function evaluations             = 9\n",
        "Number of objective gradient evaluations             = 9\n",
        "Number of equality constraint evaluations            = 9\n",
        "Number of inequality constraint evaluations          = 9\n",
        "Number of equality constraint Jacobian evaluations   = 9\n",
        "Number of inequality constraint Jacobian evaluations = 9\n",
        "Number of Lagrangian Hessian evaluations             = 8\n",
        "Total CPU secs in IPOPT (w/o function evaluations)   =      0.024\n",
        "Total CPU secs in NLP function evaluations           =      0.384\n",
        "\n",
        "EXIT: Optimal Solution Found.\n",
        "                   user           real      num           mean             mean\n",
        "                   time           time     evals       user time        real time\n",
        "       eval_f     0.008 [s]      0.008 [s]     9       0.91 [ms]        0.91 [ms]\n",
        "  eval_grad_f     0.057 [s]      0.057 [s]    10       5.68 [ms]        5.66 [ms]\n",
        "       eval_g     0.009 [s]      0.009 [s]     9       1.01 [ms]        1.01 [ms]\n",
        "   eval_jac_g     0.079 [s]      0.079 [s]    11       7.17 [ms]        7.17 [ms]\n",
        "       eval_h     0.244 [s]      0.244 [s]     9      27.13 [ms]       27.09 [ms]\n",
        " all previous     0.397 [s]      0.397 [s]\n",
        "        ipopt     0.010 [s]      0.009 [s]\n",
        "    main loop     0.407 [s]      0.406 [s]\n",
        "[0.75, 0, 0.707107, -0.353553]\n"
       ]
      }
     ],
     "prompt_number": 6
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 6
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [],
     "language": "python",
     "metadata": {},
     "outputs": []
    }
   ],
   "metadata": {}
  }
 ]
}