{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Quadratic Program (QP) Tutorial\n",
    "\n",
    "For instructions on how to run these tutorial notebooks, please see the [README](https://github.com/RobotLocomotion/drake/blob/master/tutorials/README.md)."
   ]
  },
  {
   "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",
    "\\begin{align}\n",
    "\\min_x 0.5 x^TQx + b^Tx + c\\\\\n",
    "\\text{s.t } Ex \\leq f\n",
    "\\end{align}\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 Binder/Colab. \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/namespacedrake_1_1manipulation_1_1planner.html#ab53fd2e1578db60ceb43b754671ae539) 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 easist 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.mathematicalprogram 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",
    "\\begin{align}\n",
    "(x - x_{desired})^TQ(x-x_{desired})\n",
    "\\end{align}\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": [
    "### AddL2NormCost\n",
    "You could also add the quadratic cost\n",
    "\\begin{align}\n",
    "|Ax-b|^2\n",
    "\\end{align}\n",
    "which is the squared L2 norm of the vector `Ax-b` to the program by calling `AddL2NormCost`. 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.AddL2NormCost(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": [
    "## 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.AddL2NormCost(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
}