{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# CasADi live Octave"
   ]
  },
  {
   "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": [
    "import casadi.*\n",
    "\n",
    "x = SX.sym('x');\n",
    "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:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import casadi.*\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=struct('x',[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",
    "disp(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": [
    "import casadi.*\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=struct('x',[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",
    "disp(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": [
    "import casadi.*\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",
    "[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": [
    "import casadi.*\n",
    "\n",
    "% Formulate the ODE\n",
    "x=SX.sym('x',2);\n",
    "p=SX.sym('p');\n",
    "z=1-x(2)^2;\n",
    "f=[z*x(1)-x(2)+p;x(1)];\n",
    "dae=struct('x',x,'p',p,'ode',f);\n",
    "\n",
    "% Create solver instance\n",
    "op=struct('t0',0,'tf',1);\n",
    "F=integrator('F','cvodes',dae,op);\n",
    "\n",
    "% Solve the problem\n",
    "r=F('x0',[0,1],'p',0.1);\n",
    "disp(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",
    "disp(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": [
    "import casadi.*\n",
    "\n",
    "% Formulate the DAE\n",
    "x=SX.sym('x',2);\n",
    "z=SX.sym('z');\n",
    "u=SX.sym('u');\n",
    "f=[z*x(1)-x(2)+u;x(1)];\n",
    "g=x(2)^2+z-1;\n",
    "h=x(1)^2+x(2)^2+u^2;\n",
    "dae=struct('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",
    "op=struct('t0',0,'tf',T/N);\n",
    "F=integrator('F','idas',dae,op);\n",
    "\n",
    "% Empty NLP\n",
    "w={}; lbw=[]; ubw=[];\n",
    "G={}; J=0;\n",
    "\n",
    "% Initial conditions\n",
    "Xk=MX.sym('X0',2);\n",
    "w{end+1}=Xk;\n",
    "lbw=[lbw;0;1];\n",
    "ubw=[ubw;0;1];\n",
    "\n",
    "for k=1:N\n",
    "  % Local control\n",
    "  name=['U' num2str(k-1)];\n",
    "  Uk=MX.sym(name);\n",
    "  w{end+1}=Uk;\n",
    "  lbw=[lbw;-1];\n",
    "  ubw=[ubw; 1];\n",
    "\n",
    "  % Call integrator\n",
    "  Fk=F('x0',Xk,'p',Uk);\n",
    "  J=J+Fk.qf;\n",
    "\n",
    "  % Local state\n",
    "  name=['X' num2str(k)];\n",
    "  Xk=MX.sym(name,2);\n",
    "  w{end+1}=Xk;\n",
    "  lbw=[lbw;-.25;-inf];\n",
    "  ubw=[ubw; inf; inf];\n",
    "  G{end+1}=Fk.xf-Xk;\n",
    "end\n",
    "\n",
    "% Create NLP solver\n",
    "nlp=struct('f',J,'g',vertcat(G{:}),'x',vertcat(w{:}));\n",
    "S=nlpsol('S','ipopt',nlp,struct('ipopt',struct('tol',1e-6)));\n",
    "\n",
    "% Solve NLP\n",
    "r=S('lbx',lbw,'ubx',ubw,'x0',0,'lbg',0,'ubg',0);\n",
    "disp(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": [
    "import casadi.*\n",
    "\n",
    "% Formulate the DAE\n",
    "x=SX.sym('x',2);\n",
    "z=SX.sym('z');\n",
    "u=SX.sym('u');\n",
    "f=[z*x(1)-x(2)+u;x(1)];\n",
    "g=x(2)^2+z-1;\n",
    "h=x(1)^2+x(2)^2+u^2;\n",
    "dae=struct('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",
    "op=struct('t0',0,'tf',T/N);\n",
    "F=integrator('F','idas',dae,op);\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==[0;1]);\n",
    "\n",
    "for k=1:N\n",
    "  % Local control\n",
    "  Uk=opti.variable();\n",
    "  opti.subject_to(-1<=Uk<=1);\n",
    "\n",
    "  % Call integrator\n",
    "  Fk=F('x0',Xk,'p',Uk);\n",
    "  J=J+Fk.qf;\n",
    "\n",
    "  % Local state\n",
    "  Xk=opti.variable(2);\n",
    "  opti.subject_to(Xk(1)>=-0.25);\n",
    "\n",
    "  opti.subject_to(Fk.xf==Xk);\n",
    "end\n",
    "\n",
    "opti.minimize(J);\n",
    "\n",
    "% Create NLP solver\n",
    "opti.solver('ipopt',struct,struct('tol',1e-6));\n",
    "\n",
    "% Solve NLP\n",
    "sol = opti.solve();\n",
    "sol.value(opti.x')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Octave",
   "language": "octave",
   "name": "octave"
  },
  "language_info": {
   "file_extension": ".m",
   "help_links": [
    {
     "text": "GNU Octave",
     "url": "https://www.gnu.org/software/octave/support.html"
    },
    {
     "text": "Octave Kernel",
     "url": "https://github.com/Calysto/octave_kernel"
    },
    {
     "text": "MetaKernel Magics",
     "url": "https://github.com/calysto/metakernel/blob/master/metakernel/magics/README.md"
    }
   ],
   "mimetype": "text/x-octave",
   "name": "octave",
   "version": "4.2.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}