{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# CasADi live Python3"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try out CasADi in your browser."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Hello World"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To run, click in the _code cell_ below, and hit Shift+Enter. You may need to do 'Kernel->Restart' if you paused a while..."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from casadi import *\n",
    "\n",
    "x = SX.sym(\"x\")\n",
    "print(jacobian(sin(x),x))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial examples from [paper.casadi.org](http://paper.casadi.org)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let us start out by finding the minimum of Rosenbrock's banana-valley function:\n",
    "\n",
    "\\begin{equation}\n",
    "  \\underset{\\begin{array}{c}x, y\\end{array}}{\\text{minimize}} \\quad   x^2 + 100 \\, (y-(1-x)^2)^2\n",
    "  \\label{eq:rosenbrock}\n",
    "\\end{equation}\n",
    "\n",
    "By inspection, we can see that its unique solution is $(0,1)$.\n",
    "The problem can be formulated and solved with CasADi as follows:\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from casadi import *\n",
    "\n",
    "# Symbolic representation\n",
    "x=SX.sym('x')\n",
    "y=SX.sym('y')\n",
    "z=y-(1-x)**2\n",
    "f=x**2+100*z**2\n",
    "P=dict(x=vertcat(x,y),f=f)\n",
    "\n",
    "# Create solver instance\n",
    "F=nlpsol('F','ipopt',P)\n",
    "\n",
    "# Solve the problem\n",
    "r=F(x0=[2.5,3.0])\n",
    "print(r['x'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let us reformulate the above as a constrained optimization problem, introducing a decision variable corresponding to `z` above:\n",
    "\n",
    "\\begin{equation}\n",
    "  \\begin{array}{cl}\n",
    "    \\underset{\\begin{array}{c}x, y, z\\end{array}}{\\text{minimize}} \\quad &  x^2 + 100 \\, z^2 \\\\\n",
    "    \\text{subject to} \\quad &  z+(1-x)^2 - y = 0.\n",
    "  \\end{array}\n",
    "  \\label{eq:rosenbrock_nlp}\n",
    "\\end{equation}\n",
    "\n",
    "The problem can be formulated and solved with CasADi as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from casadi import *\n",
    "\n",
    "# Formulate the NLP\n",
    "x=SX.sym('x')\n",
    "y=SX.sym('y')\n",
    "z=SX.sym('z')\n",
    "f=x**2+100*z**2\n",
    "g=z+(1-x)**2-y\n",
    "P=dict(x=vertcat(x,y,z),f=f,g=g)\n",
    "\n",
    "# Create solver instance\n",
    "F=nlpsol('F','ipopt',P)\n",
    "\n",
    "# Solve the problem\n",
    "r=F(x0=[2.5,3.0,0.75],ubg=0,lbg=0)\n",
    "print(r['x'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the paper came out, a higher-level abstraction for modeling NLPs was introduced. Here's the above example remodeled:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from casadi import *\n",
    "\n",
    "opti = Opti();\n",
    "\n",
    "x = opti.variable()\n",
    "y = opti.variable()\n",
    "z = opti.variable()\n",
    "\n",
    "opti.minimize(x**2+100*z**2)\n",
    "opti.subject_to(z+(1-x)**2-y==0)\n",
    "\n",
    "opti.set_initial(x,2.5)\n",
    "opti.set_initial(y,3.0)\n",
    "opti.set_initial(z,0.75)\n",
    "\n",
    "opti.solver('ipopt')\n",
    "sol = opti.solve()\n",
    "\n",
    "print(sol.value(x),sol.value(y))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now shift our attention to simulation and sensitivity analysis using\n",
    "the CasADi's `integrator` objects.\n",
    "Consider the following initial-value problem in ODE corresponding to a Van der Pol oscillator:\n",
    "\n",
    "\\begin{align}\n",
    "\\left\\{\n",
    "  \\begin{array}{ccll}\n",
    "    \\dot{x}_1 &=& (1-x_2^2) \\, x_1 - x_2 + p, \\quad &x_1(0)=0 \\\\\n",
    "    \\dot{x}_2 &=& x_1, \\quad &x_2(0)=1\n",
    "  \\end{array}\n",
    "  \\right.\n",
    "  \\label{eq:vdp_dae}\n",
    "\\end{align}\n",
    "\n",
    "With $p$ fixed to 0.1, we wish to solve for $x_{\\text{f}} := x(1)$.\n",
    "This can be solved as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from casadi import *\n",
    "\n",
    "# Formulate the ODE\n",
    "x=SX.sym('x',2)\n",
    "p=SX.sym('p')\n",
    "z=1-x[1]**2\n",
    "f=vertcat(z*x[0]-x[1]+p,x[0])\n",
    "dae=dict(x=x,p=p,ode=f)\n",
    "\n",
    "# Create solver instance\n",
    "options=dict(t0=0,tf=1)\n",
    "F=integrator('F','cvodes',dae,options)\n",
    "\n",
    "# Solve the problem\n",
    "r=F(x0=[0,1],p=0.1)\n",
    "print(r['xf'])\n",
    "\n",
    "# Create Jacobian function\n",
    "D=F.factory('D',['x0','p'],['jac:xf:x0'])\n",
    "\n",
    "# Solve the problem\n",
    "r=D(x0=[0,1],p=0.1)\n",
    "print(r['jac_xf_x0'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "By combining the nonlinear programing example with the embeddable integrator, we can implement the direct multiple shooting method by Bock and Plitt. We will consider a simple OCP reformulated as a DAE and with $p$ replaced by a time-varying control $u$:\n",
    "\n",
    "  \\begin{align}\n",
    "    \\displaystyle \\underset{\\begin{array}{c}x(\\cdot), z(\\cdot), u(\\cdot)\\end{array}}\n",
    "    {\\text{minimize}}\\quad &\\displaystyle \\int_{0}^{T}{ \\left( x_1(t)^2 + x_2(t)^2 + u(t)^2 \\right) \\, dt} \\\\\n",
    "    \\text{subject to} \\, \\quad\n",
    "    & \\left\\{\n",
    "    \\begin{array}{l}\n",
    "      \\dot{x}_1(t) = z(t) \\, x_1(t) - x_2(t) + u(t) \\\\\n",
    "      \\dot{x}_2(t) = x_1(t) \\\\\n",
    "      0 = x_2(t)^2 + z(t) - 1\\\\\n",
    "      -1.0 \\le u(t) \\le 1.0, \\quad x_1(t) \\ge -0.25\n",
    "    \\end{array}\n",
    "    \\right. \\quad  t \\in [0,T] \\\\\n",
    "    & x_1(0)=0, \\quad x_2(0)=1\n",
    "    \\label{eq:vdp}\n",
    "  \\end{align}\n",
    "where $x(\\cdot) \\in \\mathbb{R}^2$ is the (differential) state, $z(\\cdot) \\in \\mathbb{R}$ is the algebraic variable and $u(\\cdot) \\in \\mathbb{R}$ is the control. We let $T=10$.\n",
    "\n",
    "Our goal is to transcribe the OCP to NLP form.\n",
    "In the direct approach, the first step in this process is a parameterization of the control trajectory. For simplicity, we assume a uniformly spaced, piecewise constant control trajectory:\n",
    "$$\n",
    "  u(t) := u_k \\quad \\text{for $t \\in [t_k, t_{k+1}), \\quad k=0, \\ldots, N-1$}\n",
    "  \\quad \\text{with $t_k := k \\, T / N$.}\n",
    "$$\n",
    "\n",
    "With the control fixed over one interval, we can use an integrator to reformulate the problem from continuous time to discrete time. Since we now have a DAE, we introduce an algebraic variable $z$ and the corresponding algebraic equation $g$ and use IDAS instead of CVODES. We also introduce a quadrature for calculating the contributions to the cost function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from casadi import *\n",
    "\n",
    "# Formulate the DAE\n",
    "x=SX.sym('x',2)\n",
    "z=SX.sym('z')\n",
    "u=SX.sym('u')\n",
    "f=vertcat(z*x[0]-x[1]+u,x[0])\n",
    "g=x[1]**2+z-1\n",
    "h=x[0]**2+x[1]**2+u**2\n",
    "dae=dict(x=x,p=u,ode=f,z=z,alg=g,quad=h)\n",
    "\n",
    "# Create solver instance\n",
    "T = 10. # end time\n",
    "N = 20 # discretization\n",
    "options=dict(t0=0,tf=T/N)\n",
    "F=integrator('F','idas',dae,options)\n",
    "\n",
    "# Empty NLP\n",
    "w=[]; lbw=[]; ubw=[]\n",
    "G=[]; J=0\n",
    "\n",
    "# Initial conditions\n",
    "Xk=MX.sym('X0',2)\n",
    "w+=[Xk]\n",
    "lbw+=[0,1]\n",
    "ubw+=[0,1]\n",
    "\n",
    "for k in range(1,N+1):\n",
    "  # Local control\n",
    "  name='U'+str(k-1)\n",
    "  Uk=MX.sym(name)\n",
    "  w+=[Uk]\n",
    "  lbw+=[-1]\n",
    "  ubw+=[ 1]\n",
    "\n",
    "  # Call integrator\n",
    "  Fk=F(x0=Xk,p=Uk)\n",
    "  J+=Fk['qf']\n",
    "\n",
    "  # Local state\n",
    "  name='X'+str(k)\n",
    "  Xk=MX.sym(name,2)\n",
    "  w+=[Xk]\n",
    "  lbw+=[-.25,-inf]\n",
    "  ubw+=[ inf, inf]\n",
    "  G+=[Fk['xf']-Xk]\n",
    "\n",
    "# Create NLP solver\n",
    "nlp=dict(f=J,g=vertcat(*G),x=vertcat(*w))\n",
    "S=nlpsol('S','ipopt',nlp,dict(ipopt=dict(tol=1e-6)))\n",
    "\n",
    "# Solve NLP\n",
    "r=S(lbx=lbw,ubx=ubw,x0=0,lbg=0,ubg=0)\n",
    "print(r['x'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Again, we note that the same can be achieved using a higher-le}vel abstraction for modeling NLPs:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from casadi import *\n",
    "\n",
    "# Formulate the DAE\n",
    "x=SX.sym('x',2)\n",
    "z=SX.sym('z')\n",
    "u=SX.sym('u')\n",
    "f=vertcat(z*x[0]-x[1]+u,x[0])\n",
    "g=x[1]**2+z-1\n",
    "h=x[0]**2+x[1]**2+u**2\n",
    "dae=dict(x=x,p=u,ode=f,z=z,alg=g,quad=h)\n",
    "\n",
    "# Create solver instance\n",
    "T = 10. # end time\n",
    "N = 20 # discretization\n",
    "options=dict(t0=0,tf=T/N)\n",
    "F=integrator('F','idas',dae,options)\n",
    "\n",
    "opti = Opti()\n",
    "\n",
    "# Build up objective\n",
    "J=0\n",
    "\n",
    "# Initial conditions\n",
    "Xk = opti.variable(2)\n",
    "opti.subject_to(Xk==vertcat(0,1))\n",
    "\n",
    "for k in range(1,N+1):\n",
    "  # Local control\n",
    "  Uk = opti.variable()\n",
    "  opti.subject_to(opti.bounded(-1,Uk,1))\n",
    "\n",
    "  # Call integrator\n",
    "  Fk=F(x0=Xk,p=Uk)\n",
    "  J+=Fk['qf']\n",
    "\n",
    "  # Local state\n",
    "  Xk = opti.variable(2)\n",
    "  opti.subject_to(Xk[0]>=-0.25)\n",
    "\n",
    "  opti.subject_to(Fk['xf']==Xk)\n",
    "\n",
    "opti.minimize(J)\n",
    "\n",
    "# Create NLP solver\n",
    "opti.solver('ipopt',{},dict(tol=1e-6))\n",
    "\n",
    "# Solve NLP\n",
    "sol = opti.solve()\n",
    "print (sol.value(opti.x))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}