{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Quadratic Program (QP) Tutorial\n",
    "For instructions on how to run these tutorial notebooks, please see the [index](./index.ipynb).\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Important Note\n",
    "Please refer to [mathematical program tutorial](./mathematical_program.ipynb) for constructing and solving a general optimization program in Drake."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A (convex) quadratic program (QP) is a special type of convex optimization. Its cost function is a convex quadratic function. Its constraints are linear, same as the constraints in linear program. A (convex) quadratic program has the following form\n",
    "\n",
    "$\\begin{aligned}\n",
    "\\min_x \\quad & 0.5 x^TQx + b^Tx + c\\\\\n",
    "\\text{s.t.} \\quad & Ex \\leq f\n",
    "\\end{aligned}$\n",
    "\n",
    "where `Q` is a positive semidefinite matrix.\n",
    "\n",
    "A quadratic program can be solved by many different solvers. Drake supports some solvers including OSQP, SCS, Gurobi, MOSEK™, etc. Please see our [Doxygen page]( https://drake.mit.edu/doxygen_cxx/group__solvers.html) for a complete list of supported solvers. Note that some commercial solvers (such as Gurobi and MOSEK™) are not included in the pre-compiled Drake binaries, and therefore not on Deepnote/Colab/Binder. \n",
    "    \n",
    "Drake's API supports multiple functions to add quadratic cost and linear constraints. We briefly go through some of the functions in this tutorial. For a complete list of functions, please check our [Doxygen](https://drake.mit.edu/doxygen_cxx/classdrake_1_1solvers_1_1_mathematical_program.html).\n",
    "\n",
    "There are many applications of quadratic programs in robotics, for example, we can solve differential inverse kinematics problem as a QP, see [DifferentialInverseKinematics](https://drake.mit.edu/doxygen_cxx/group__planning__kinematics.html#gac36361db531cfc7f707ef52db81375e9) for more details. For more examples, check out [Underactuated Robotics code repo](https://github.com/RussTedrake/underactuated)\n",
    "\n",
    "## Add quadratic cost\n",
    "\n",
    "### AddQuadraticCost function\n",
    "\n",
    "The easiest way to add a quadratic cost is to call the `AddQuadraticCost` function. In the following code snippet, we first construct a program with 2 decision variables, and then show how to call the `AddQuadraticCost` function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pydrake.solvers import MathematicalProgram, Solve\n",
    "import numpy as np\n",
    "\n",
    "# Create an empty MathematicalProgram named prog (with no decision variables,\n",
    "# constraints or costs)\n",
    "prog = MathematicalProgram()\n",
    "# Add two decision variables x[0], x[1].\n",
    "x = prog.NewContinuousVariables(2, \"x\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can call `AddQuadraticCost(expression)` to add the quadratic cost, where `expression` is a symbolic expression representing a quadratic cost."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add a symbolic quadratic expression as the quadratic cost.\n",
    "cost1 = prog.AddQuadraticCost(x[0]**2 + 2*x[0]*x[1] + x[1]**2 + 3*x[0] + 4)\n",
    "# The newly added cost is returned as cost1\n",
    "print(cost1)\n",
    "# The newly added cost is stored inside prog.\n",
    "print(prog.quadratic_costs()[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If we call `AddQuadraticCost` again, the total cost of `prog` is the summation of all the added costs. You can see that `prog.quadratic_costs()` has two entries. And the total cost of `prog` is `cost1 + cost2`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add another quadratic cost to prog.\n",
    "cost2 = prog.AddQuadraticCost(x[1]*x[1] + 3)\n",
    "print(f\"The number of quadratic costs in prog: {len(prog.quadratic_costs())}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If you know the coefficients of the quadratic cost `Q, b, c`, you could also add the cost without using the symbolic expression, as shown in the following code snippet"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add the cost x[0]*x[0] + x[0]*x[1] + 1.5*x[1]*x[1] + 2*x[0] + 4*x[1] + 1\n",
    "cost3 = prog.AddQuadraticCost(Q=[[2, 1], [1, 3]], b=[2, 4], c=1, vars=x)\n",
    "print(f\"cost 3 is {cost3}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### AddQuadraticErrorCost\n",
    "You could also add the quadratic cost\n",
    "\n",
    "$\\begin{aligned}\n",
    "(x - x_{desired})^TQ(x-x_{desired})\n",
    "\\end{aligned}$\n",
    "\n",
    "to the program by calling `AddQuadraticErrorCost`. Here is the code example"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Adds the cost (x - [1;2])' * Q * (x-[1;2])\n",
    "cost4 = prog.AddQuadraticErrorCost(Q=[[1, 2],[2, 6]], x_desired=[1,2], vars=x)\n",
    "print(f\"cost4 is {cost4}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Add2NormSquaredCost\n",
    "You could also add the quadratic cost\n",
    "\n",
    "$\\begin{aligned}\n",
    "|Ax-b|^2\n",
    "\\end{aligned}$\n",
    "\n",
    "which is the squared L2 norm of the vector `Ax-b` to the program by calling `Add2NormSquaredCost`. Here is the code example"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Adds the squared norm of (x[0]+2*x[1]-2, x[1] - 3) to the program cost.\n",
    "cost5 = prog.Add2NormSquaredCost(A=[[1, 2], [0, 1]], b=[2, 3], vars=x)\n",
    "print(f\"cost5 is {cost5}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Add linear cost\n",
    "\n",
    "You could also add linear costs to a quadratic program. For an introduction on the different APIs on adding a linear cost, please refer to our [linear programming tutorial](./linear_program.ipynb). Here is an example"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Adds a linear cost to the quadratic program\n",
    "cost6 = prog.AddLinearCost(x[0] + 3 * x[1] + 1)\n",
    "print(f\"cost6 is {cost6}\")\n",
    "print(f\"Number of linear costs in prog: {len(prog.linear_costs())}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Add linear constraints\n",
    "\n",
    "To add linear constraints into quadratic program, please refer to the section `Add linear constraints` in our [linear programming tutorial](./linear_program.ipynb). Here is a brief example"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "constraint1 = prog.AddLinearConstraint(x[0] + 3*x[1] <= 5)\n",
    "# Adds the constraint 1 <= x[0] <= 5 and 1 <= x[1] <= 5\n",
    "constraint2 = prog.AddBoundingBoxConstraint(1, 5, x)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Convexity of the cost\n",
    "Drake checks the convexity of every added quadratic cost, by checking whether the Hessian of each quadratic cost is positive semidefinite or not. Currently the user can solve a QP with a convex solver (Gurobi/MOSEK™/OSQP/CLP/SCS etc) only if <em> every </em> quadratic cost is convex.\n",
    "\n",
    "It takes some computation to check if a quadratic cost is convex. If your application requires solving the QP as fast as possible (for example, solving a QP within the robot control loop), then it makes sense to bypass this convexity check. To do so, if you know the convexity of your quadratic cost, you can set the `is_convex` flag in `AddQuadraticCost` function; Drake won't compute whether the cost is convex or not when the `is_convex` flag is set. In the following example we show how to set this `is_convex` flag."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Call AddQuadraticCost with the is_convex flag unspecified. Drake will check the convexity\n",
    "# of this cost.\n",
    "cost7 = prog.AddQuadraticCost(x[0]**2 + 3 * x[1]**2 + x[0] * x[1] + 2 * x[0])\n",
    "print(f\"Is the cost {cost7} convex? {cost7.evaluator().is_convex()}\")\n",
    "cost8 = prog.AddQuadraticCost(x[0]**2 + 3 * x[1]**2 + 4*x[0]*x[1])\n",
    "print(f\"Is the cost {cost8} convex? {cost8.evaluator().is_convex()}\")\n",
    "\n",
    "# Call AddQuadraticCost with specified is_convex flag. Drake won't check the convexity of\n",
    "# the quadratic cost when this flag is specified.\n",
    "cost9 = prog.AddQuadraticCost(x[0]**2 + 4 * x[1]**2 + 3 * x[0]*x[1], is_convex=True)\n",
    "print(f\"Is the cost {cost9} convex? {cost9.evaluator().is_convex()}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## A complete code example\n",
    "Here we show a complete example to construct and solve a QP"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "prog = MathematicalProgram()\n",
    "x = prog.NewContinuousVariables(3, \"x\")\n",
    "prog.AddQuadraticCost(x[0] * x[0] + 2 * x[0] + 3)\n",
    "# Adds the quadratic cost on the squared norm of the vector\n",
    "# (x[1] + 3*x[2] - 1, 2*x[1] + 4*x[2] -4)\n",
    "prog.Add2NormSquaredCost(A = [[1, 3], [2, 4]], b=[1, 4], vars=[x[1], x[2]])\n",
    "\n",
    "# Adds the linear constraints.\n",
    "prog.AddLinearEqualityConstraint(x[0] + 2*x[1] == 5)\n",
    "prog.AddLinearConstraint(x[0] + 4 *x[1] <= 10)\n",
    "# Sets the bounds for each variable to be within [-1, 10]\n",
    "prog.AddBoundingBoxConstraint(-1, 10, x)\n",
    "\n",
    "# Solve the program.\n",
    "result = Solve(prog)\n",
    "print(f\"optimal solution x: {result.GetSolution(x)}\")\n",
    "print(f\"optimal cost: {result.get_optimal_cost()}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For more details on quadratic programming, you could refer to\n",
    "\n",
    "[Quadratic Programming wiki](https://en.wikipedia.org/wiki/Quadratic_programming)\n",
    "\n",
    "[Numerical Optimization by J. Nocedal and S.Wright](http://www.apmath.spbu.ru/cnsa/pdf/monograf/Numerical_Optimization2006.pdf)"
   ]
  }
 ],
 "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.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}