{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Updating costs and constraints in MathematicalProgram\n",
    "For instructions on how to run these tutorial notebooks, please see the [index](./index.ipynb).\n",
    "\n",
    "Often cases after we solve an optimization problem, we want to tweak its constraints and costs slightly, and then resolve the updated problem. One use case is in model predictive control, where in each time instance we solve a new optimization problem, whose constraints/costs are just slightly different from the one in the previous time instance. \n",
    "\n",
    "Instead of constructing a new `MathematicalProgram` object, we could update the constraints/costs in the old `MathematicalProgram` object, and then solve the updated problem. To do so, many constraints/costs object provide an \"update\" function. In this tutorial we show how to update certain types of constraints/costs\n",
    "\n",
    "## Updating `LinearCost`\n",
    "For a linear cost $a^Tx + b$, we could call [`LinearCost.UpdateCoefficients()`](https://drake.mit.edu/pydrake/pydrake.solvers.html#pydrake.solvers.LinearCost.UpdateCoefficients) function to update the linear coefficient `a` vector or the constant term `b`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pydrake.solvers import MathematicalProgram, Solve\n",
    "import numpy as np\n",
    "\n",
    "prog = MathematicalProgram()\n",
    "x = prog.NewContinuousVariables(2)\n",
    "cost1 = prog.AddLinearCost(2*x[0] + 3 * x[1] + 2)\n",
    "print(f\"original cost: {cost1}\")\n",
    "prog.AddBoundingBoxConstraint(-1, 1, x)\n",
    "result = Solve(prog)\n",
    "print(f\"optimal solution: {result.GetSolution(x)}\")\n",
    "print(f\"original optimal cost: {result.get_optimal_cost()}\")\n",
    "\n",
    "# Now update the cost to 3x[0] - 4x[1] + 5\n",
    "cost1.evaluator().UpdateCoefficients(new_a=[3, -4], new_b=5)\n",
    "print(f\"updated cost: {cost1}\")\n",
    "# Solve the optimization problem again with the updated cost.\n",
    "result = Solve(prog)\n",
    "print(f\"updated optimal solution: {result.GetSolution(x)}\")\n",
    "print(f\"updated optimal cost: {result.get_optimal_cost()}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Updating `QuadraticCost`\n",
    "For a quadratic cost in the form $0.5x^TQx + b'x + c$, we can also call [`QuadraticCost.UpdateCoefficients`](https://drake.mit.edu/pydrake/pydrake.solvers.html#pydrake.solvers.QuadraticCost.UpdateCoefficients) to update its $Q, b, c$ terms"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "prog = MathematicalProgram()\n",
    "x = prog.NewContinuousVariables(2)\n",
    "cost1 = prog.AddQuadraticCost(x[0]**2 + 2 * x[1]**2 + x[0]*x[1] + 3*x[0] + 5)\n",
    "print(f\" original cost: {cost1}\")\n",
    "cost1.evaluator().UpdateCoefficients(new_Q=[[1., 2], [2., 4]], new_b=[1, 2.], new_c= 4)\n",
    "print(f\" updated cost: {cost1}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Updating the bounds for any constraint\n",
    "For any constraint $lower \\le f(x) \\le upper$, we can update its bounds by\n",
    "- [`Constraint.UpdateLowerBound(new_lb)`](https://drake.mit.edu/pydrake/pydrake.solvers.html#pydrake.solvers.LinearConstraint.UpdateLowerBound) to change its lower bound to `new_lb`.\n",
    "- [`Constraint.UpdateUpperBound(new_ub)`](https://drake.mit.edu/pydrake/pydrake.solvers.html#pydrake.solvers.LinearConstraint.UpdateUpperBound) to change its upper bound to `new_ub`.\n",
    "- [`Constraint.set_bounds(new_lb, new_ub)`](https://drake.mit.edu/pydrake/pydrake.solvers.html#pydrake.solvers.LinearConstraint.set_bounds) up change both its lower and upper bounds"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "prog = MathematicalProgram()\n",
    "x = prog.NewContinuousVariables(2)\n",
    "constraint1 = prog.AddLinearConstraint(x[0] + 3 * x[1] <= 2)\n",
    "print(f\"original constraint: {constraint1}\")\n",
    "constraint1.evaluator().UpdateLowerBound([-1])\n",
    "print(f\"updated constraint: {constraint1}\")\n",
    "constraint1.evaluator().UpdateUpperBound([3])\n",
    "print(f\"updated constraint: {constraint1}\")\n",
    "constraint1.evaluator().set_bounds(new_lb=[-5], new_ub=[10])\n",
    "print(f\"updated constraint: {constraint1}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Update linear constraint coefficients and bounds\n",
    "For a linear constraint $lower \\le Ax \\le upper$, we can call [`LinearConstraint.UpdateCoefficients(new_A, new_lb, new_ub)`](https://drake.mit.edu/pydrake/pydrake.solvers.html#pydrake.solvers.LinearConstraint.UpdateCoefficients) to update the constraint as $new_{lb} \\le new_A* x\\le new_ub$.\n",
    "\n",
    "For a linear equality constraint $Ax = b$, we can call [`LinearEqualityConstraint.UpdateCoefficients(Aeq, beq)`](https://drake.mit.edu/pydrake/pydrake.solvers.html#pydrake.solvers.LinearEqualityConstraint.UpdateCoefficients) to update the constraint to $\\text{Aeq}*x=\\text{beq}$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "prog = MathematicalProgram()\n",
    "x = prog.NewContinuousVariables(2)\n",
    "linear_constraint = prog.AddLinearConstraint(3 * x[0] + 4 * x[1] <= 5)\n",
    "linear_eq_constraint = prog.AddLinearConstraint(5 * x[0] + 2 * x[1] == 3)\n",
    "print(f\"original linear constraint: {linear_constraint}\")\n",
    "linear_constraint.evaluator().UpdateCoefficients(new_A = [[1, 3]], new_lb=[-2], new_ub=[3])\n",
    "print(f\"updated linear constraint: {linear_constraint}\")\n",
    "\n",
    "print(f\"original linear equality constraint: {linear_eq_constraint}\")\n",
    "linear_eq_constraint.evaluator().UpdateCoefficients(Aeq=[[3, 4]], beq=[2])\n",
    "print(f\"updated linear equality constraint: {linear_eq_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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}