{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Inequality constrained problems with the nullspace optimizer\n",
    "\n",
    "In this notebook, we investigate the use of the null space algorithm for solving equality **and inequality** optimization problems of the form\n",
    "$$\n",
    "\\newcommand{\\x}{{\\bf x}}\n",
    "\\newcommand{\\g}{{\\bf g}}\n",
    "\\newcommand{\\<}{\\leqslant}\n",
    "\\newcommand{\\R}{\\mathbb{R}}\n",
    "\\newcommand{\\h}{{\\bf h}}\n",
    "\\begin{aligned}\n",
    "    \\min_{\\x\\in V}&  \\quad J(\\x)\\\\\n",
    "    \\textrm{s.t.} & \\left\\{\\begin{aligned}\n",
    " \\g(\\x)&=0\\\\\n",
    " \\h(\\x)&\\<0,\n",
    "        \\end{aligned}\\right.\n",
    "\\end{aligned}\n",
    "\\label{eqn:equation}\n",
    "\\tag{1}\n",
    "$$\n",
    "\n",
    "where $V$ is the optimization set, $J\\,:\\,V\\to \\R$ is the objective function and \n",
    "$\\g\\,:\\,V\\rightarrow \\R^p$ and $\\h\\,:\\,V\\rightarrow \\R^q$ are respectively the\n",
    "respectively the equality and inequality constraint functionals. \n",
    "\n",
    "\n",
    "The basis of the method is to solve an Ordinary Differential Equation (so-called ''null-space gradient flow''),\n",
    "\\begin{equation}\n",
    "    \\label{eqn:ODE}\n",
    "    \\dot{x}(t) =-\\alpha_J \\xi_J(x(t))-\\alpha_C\\xi_C(x(t)),\n",
    "\\end{equation}\n",
    "which is able to solve the optimization problem $\\eqref{eqn:equation}$.\n",
    "\n",
    "The direction\n",
    "$\\xi_J(x(t))$ is called the null space direction, it is the ''best'' locally feasible\n",
    "descent direction for minimizing $J$ and respecting the constraints. \n",
    "The direction\n",
    "$\\xi_C(x(t))$ is called the range space direction, it makes the violated constraints\n",
    "better satisfied and corrects unfeasible initializations.\n",
    "\n",
    "$$\n",
    "\\newcommand{\\>}{\\geqslant}\n",
    "\\newcommand{\\DD}{\\mathrm{D}}\n",
    "$$\n",
    " The null space direction \n",
    " $\\xi_J(x)$ can be interpreted as a generalization\n",
    "of the gradient in presence of equality and inequality constraints.\n",
    "It is defined as positively proportional to the solution of the following \n",
    "minimization problem:\n",
    "$$\\begin{aligned}\n",
    "\t&  \\min_{\\xi\\in V} \\quad\\DD J(x)\\xi\\\\\n",
    "\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t& \\textrm{s.t. }\n",
    "\t\t\\left\\{\\begin{aligned}\n",
    "\t\t\t\t\\DD\\g(x)\\xi &= 0\\\\\n",
    "\t\t\t\t\\DD\\h_{\\widetilde{I\\,}(x)}(x)\\xi& \\<0\\\\\n",
    " ||\\xi||_V  & \\< 1.\n",
    "\t\t\t\\end{aligned}\\right.\n",
    "\t\\end{aligned}\n",
    "$$\n",
    "where\n",
    "$\\widetilde{I\\,}(x)$ is the set of violated or saturated constraints:\n",
    "$$ \\widetilde{I\\,}(x):=\\{i\\in \\{1,\\dots,q\\}\\,|\\, h_i(x)\\>0\\}.$$\n",
    "This minimization problem always has a solution. When the number of constraints is not too large, \n",
    "$\\xi_J(x)$ can be computed efficiently by mean\n",
    "of a dual subproblem.\n",
    "\n",
    " The range space direction $\\xi_C(x)$ is a Gauss-Newton direction for cancelling the violated constraints\n",
    " of $\\widetilde{I\\,}(x)$:\n",
    "$$\n",
    "\\newcommand{\\CC}{\\mathrm{C}}\n",
    "\\xi_C(x):=\\DD\\CC_{\\widetilde{I\\,}(x)}^T(\\DD\\CC_{\\widetilde{I\\,}(x)} \\DD\\CC_{\\widetilde{I\\,}(x)}^T)^{-1}\\CC_{\\widetilde{I\\,}(x)}(x),\n",
    "\\textrm{ with }\n",
    " \\CC_{\\widetilde{I\\,}}(x):=\\begin{bmatrix} \\g(x) & \\h_{\\widetilde{I\\,}(x)}(x)\\end{bmatrix}^T,\\, \\h_{\\widetilde{I\\,}(x)}(x):=(h_i(x))_{i\n",
    " \\in \\widetilde{I\\,}(x)}.\n",
    "$$\n",
    "It is orthogonal to $\\xi_J(x)$ and satisfies $\\DD\\CC_{\\widetilde{I\\,}(x)}\\xi_C(x)=\\CC_{\\widetilde{I\\,}(x)}(x)$ which \n",
    "entails an exponential decrease of the violation of the constraints at rate at least $e^{-\\alpha_C t}$ for the solution\n",
    "$x(t)$ of \\eqref{eqn:equation}:\n",
    "$$\\forall t\\>0,\\,  \\g(x(t))= \\g(x(0)) e^{-\\alpha_C t}\\textrm{ and }\n",
    "\\h(x(t)) \\< \\max(0,\\h(x(0))e^{-\\alpha_C t}.$$\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# A first optimization program\n",
    "\n",
    "$$\\newcommand{\\R}{\\mathbb{R}}$$\n",
    "We consider the following optimization program in $\\R^2$:\n",
    "\n",
    "$$\\newcommand{\\<}{\\leq}\n",
    "\\begin{aligned} \\min_{(x_0,x_1)\\in\\mathbb{R}^2} & \\quad (x_0+1)^2+(x_1+1)^2 \\\\\n",
    "s.t. &\\quad  \\left\\{ \\begin{aligned} x_0^2+x_1^2-1 & \\< 0\\\\\n",
    "                             x_0+x_1-1 & \\< 0 \\\\\n",
    "                             -x_1-0.7 & \\<0\n",
    "                             \\end{aligned}\\right.\n",
    "\\end{aligned}$$\n",
    " "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In order to solve the optimization problem, we use the function `nlspace_solve` whose prototype is \n",
    "```python\n",
    "from nullspace_optimizer import nlspace_solve\n",
    "\n",
    "# Define problem\n",
    "\n",
    "results = nlspace_solve(problem: Optimizable, params=None, results=None)\n",
    "```\n",
    "The input variables are \n",
    "-  `problem` : an `Optimizable` object described below. This variable contains all the information about the optimization problem to solve (objective and constraint functions, derivatives...)\n",
    "- `params`  : (optional) a dictionary containing algorithm parameters.\n",
    "\n",
    "- `results` : (optional) a previous output of the `nlspace_solve` function.         The optimization will then keep going from the last input of           the dictionary `results['x'][-1]`. This is useful when one needs to restart an optimization after an interruption.\n",
    "\n",
    "The optimization routine `nlspace_solve` returns the dictionary `opt_results` which contains various information about the optimization path, including the values of the optimization variables `results['x']`.\n",
    "    \n",
    "    \n",
    " In our particular optimization test case in $\\R^n$ with $n=2$, we use the  `EuclideanOptimizable` class which inherits `Optimizable` which simplifies the definition of the optimization program (the inner product is specified by default). \n",
    "We allow the user to specify the initialization in the constructor `__init__`.\n",
    "\n",
    "Fill in the definition below with the right values for solving the above optimization program."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from nullspace_optimizer import *\n",
    "\n",
    "\n",
    "class problemeSimple(EuclideanOptimizable):\n",
    "    def __init__(self, xinit):\n",
    "        super().__init__(n=2)\n",
    "        self.xinit = xinit\n",
    "\n",
    "        self.nconstraints = 0\n",
    "        self.nineqconstraints = # To fill\n",
    "\n",
    "    def x0(self):\n",
    "        return self.xinit\n",
    "\n",
    "    def J(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def G(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def H(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def dJ(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def dG(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def dH(self, x):\n",
    "        return #to fill\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let us draw the optimization domain and the objective function with the function `draw` provided by the package:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import nullspace_optimizer.examples.draw as draw\n",
    "\n",
    "draw.drawProblem(problemeSimple([0, 0]), [-1.5, 1.5], [-1.5, 1.5])\n",
    "# drawProblem(problem : EuclideanOptimizable, [xmin,xmax], [ymin,ymax] )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The white area is the feasible domain. \n",
    "\n",
    "\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "3\\. Solve the optimization program with the initializations $[1.25,0]$, $[0,-1.2]$, $[1,-1]$, $[0.7,1.2]$, $[-1,1]$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "def run_problems(**other_params):\n",
    "    xinits =  #to fill\n",
    "    problems = [problemeSimple(xinit=x0) for x0 in xinits]\n",
    "    params = {'dt': 0.05, 'alphaC': 0.2, 'alphaJ': 1, 'maxtrials': 1, 'debug': -1}\n",
    "    params.update(other_params)\n",
    "    return [nlspace_solve(pb, params) for pb in problems]\n",
    "\n",
    "results = run_problems()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Use `drawData` to display the optimized trajectories."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "draw.drawProblem(problemeSimple([0, 0]), [-1.5, 1.5], [-1.5, 1.5])\n",
    "for i, r in enumerate(results):\n",
    "    draw.drawData(r, f'x{i}', f'C{i}', x0=True, xfinal=True, initlabel=None)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If one wishes to understand the nature of the trajectories, it is useful to look at the evolution of the Lagrange multipliers along the optimization trajectories. These are saved in `results['muls']`. \n",
    "\n",
    "Use the following code to display the multipliers and comment with respect to the trajectories. The variable `s` is the arc length of the optimization path.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "draw.figure()\n",
    "draw.drawMuls(results[0], 'x1')\n",
    "draw.legend()\n",
    "draw.title('Lagrange multipliers for trajectory x1')\n",
    "draw.show()\n",
    "draw.figure()\n",
    "draw.drawMuls(results[1], 'x2')\n",
    "draw.legend()\n",
    "draw.title('Lagrange multipliers for trajectory x2')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "How do you interpret:\n",
    "- the fact that $\\mu_1,\\mu_2=0$ in the beginning of the optimization\n",
    "- the final discontinuities \n",
    "- are the value displayed below consistent ?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "for x in results:\n",
    "    print(x['muls'][-1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "More exploration:\n",
    "- Change the value of the parameters $\\alpha_J,\\alpha_C$ and $\\Delta t$. How the optimization trajectories are modified ?\n",
    "- Solve the following problem:\n",
    "$$\\begin{aligned} \\min_{(x_0,x_1)\\in\\mathbb{R}^2} & \\quad x_0 \\\\\n",
    "s.t. &\\quad  \\left\\{ \\begin{aligned} 0.5^2-x_0^2-x_1^2 & \\< 0\\\\\n",
    "                             -1-x_0 & \\< 0\n",
    "                             \\end{aligned}\\right.\n",
    "\\end{aligned}$$\n",
    "Test with the initializations $[1.25,0], [0,-1.2], [1,0.3], [0.7,-0.2], [-1.3,1]$. Comment on the evolution of the Lagrange multipliers."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class problemeSimple2(EuclideanOptimizable):\n",
    "    def __init__(self, xinit):\n",
    "        super().__init__(n=2)\n",
    "        self.xinit = xinit\n",
    "\n",
    "        self.nconstraints = #to fill\n",
    "        self.nineqconstraints = #to fill\n",
    "\n",
    "    def x0(self):\n",
    "        return self.xinit\n",
    "\n",
    "    def J(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def G(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def H(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def dJ(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def dG(self, x):\n",
    "        return #to fill\n",
    "\n",
    "    def dH(self, x):\n",
    "        return # to fill\n",
    "\n",
    "def run_problems(**other_params):\n",
    "    xinits = #to fill\n",
    "    problems = [problemeSimple2(xinit=x0) for x0 in xinits]\n",
    "    params = {'dt': 0.01, 'alphaC': 1, 'alphaJ': 1, 'maxtrials': 1, 'debug': -1}\n",
    "    params.update(other_params)\n",
    "    return [nlspace_solve(pb, params) for pb in problems]\n",
    "\n",
    "\n",
    "results = run_problems()\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import nullspace_optimizer.examples.draw as draw\n",
    "draw.ion()\n",
    "draw.drawProblem(problemeSimple2([0, 0]), [-1.5, 1.5], [-1.5, 1.5])\n",
    "for i, r in enumerate(results):\n",
    "    draw.drawData(r, f'x{i}', f'C{i}', x0=True, xfinal=True, initlabel=None)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Display the Lagrange multiplier associated to the trajectory `x0`, `x2` and comment on their evolution."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "draw.drawMuls(results[0],'x0')\n",
    "import matplotlib.pyplot as plt\n",
    "plt.figure()\n",
    "draw.drawMuls(results[2],'x2')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What would have happened if the algorithm would have kept projected the trajectories on the constraint ? "
   ]
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}