forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
305 lines
11 KiB
305 lines
11 KiB
{
|
|
"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/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 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
|
|
}
|