mathematical_program.org

#+title: MathematicalProgram Tutorial
#+setupfile: https://fniessen.github.io/org-html-themes/org/theme-readtheorg.setup

* Background
Many engineering problems can be formulated as mathematical optimization
problems, and solved by numerical solvers. A generic mathematical
optimization problem can be formulated as

$\begin{aligned} \begin{array}{rl}  \min_x \; & f(x)  \\\text{subject to} \; & x \in\mathcal{S}  \end{array}  \qquad  \boxed{  \begin{array}{ll}  \text{The real-valued decision variable is} &x\\  \text{The real-valued cost function is} &f(x)\\  \text{The constraint set is} &\mathcal{S}\\  \text{The optimal } x \text{ that minimizes the cost function is} &x^*  \end{array}  } \end{aligned}$

where $x$ is the real-valued decision variable(s), $f(x)$ is the
real-valued /cost function/, $\mathcal{S}$ is the constraint set for
$x$. Our goal is to find the optimal $x^*$ within the constraint set
$\mathcal{S}$, such that $x^*$ minimizes the cost function $f(x)$.

For example, the following optimization problem determines the value of
$x$ that minimizes $x^3 + 2x + 1$ subject to $x \ge 1$.
$\begin{aligned} \begin{array}{rl} \min_x\;&x^3 + 2x + 1\\ \text{subject to}\;\;&x \ge 1 \end{array} \quad \boxed{  \begin{array}{ll}  \text{The real-valued decision variable is} & x\\  \text{The real-valued cost function }f(x) \text{ is} & x^3 + 2x + 1\\  \text{The set }\mathcal{S} \text{ of constraints is} & x \ge 1\\  \text{The value that minimizes the cost function is} & x^* = 1  \end{array} } \end{aligned}$

In general, how an optimization problem is solved depends on its
categorization (categories include Linear Programming, Quadratic
Programming, Mixed-integer Programming, etc.). Categorization depends on
properties of both the cost function $f(x)$ and the constraint set
$\mathcal{S}$. For example, if the cost function $f(x)$ is a linear
function of $x$, and the constraint $\mathcal{S}$ is a linear set
$\mathcal{S} = \{x | Ax\le b\}$, then we have a /linear programming/
problem, which is efficiently solved with certain solvers.

There are multiple solvers for each category of optimization problems,
but each solver has its own API and data structures. Frequently, users
need to rewrite code when they switch solvers. To remedy this, Drake
provides a common API through the /MathematicalProgram/ class. In
addition to avoiding solver-specific code, the constraint and cost
functions can be written in symbolic form (which makes code more
readable). In these ways, Drake's MathematicalProgram is akin to
[[https://yalmip.github.io/][YALMIP]] in MATLAB or
[[https://github.com/JuliaOpt/JuMP.jl][JuMP]] in Julia, and we support
both Python and C++. Note: Drake supports many
[[https://drake.mit.edu/doxygen_cxx/group__solvers.html][solvers]] (some
are open-source and some require a license).

Drake can formulate and solve the following categories of optimization
problems
 + Linear programming
 + Quadratic programming
 + Second-order cone programming
 + Nonlinear nonconvex programming
 + Semidefinite programming
 + Sum-of-squares programming
 + Mixed-integer programming (mixed-integer linear programming, mixed-integer quadratic programming, mixed-integer second-order cone programming).
 + Linear complementarity problem

This tutorial provides the basics of Drake's MathematicalProgram.
Advanced tutorials are available at the [[*Advanced tutorials][bottom]] of this document.

* Basics of MathematicalProgram class
Drake's MathematicalProgram class contains the mathematical formulation of an optimization problem,
namely the decision variables $x$, the cost function $f(x)$, and the constraint set $\mathcal{S}$.

** Initialize a MathematicalProgram object
To initialize this class, first create an empty MathematicalProgram as

#+begin_src cpp
  #include "drake/solvers/mathematical_program.h"
  // ...
  drake::solvers::MathematicalProgram prog;
  // ...
#+end_src

** Adding decision variables
Shown below, the function ~NewContinuousVariables~ adds two new continuous decision variables to ~prog~.  The newly added variables are returned as ~x~ (~VectorXDecisionVariable~).
Note the range of the variable is a continuous set, as opposed to binary variables which only take discrete value 0 or 1.

The default names of the variable in *x* are "x(0)" and "x(1)".  The next line prints the default names and types in ~x~,
whereas the second line prints the symbolic expression "1 + 2x[0] + 3x[1] + 4x[1]".

#+begin_src cpp
  auto x = prog.NewContinuousVariables(2);
  std::cout << x << "\n";
  /**
   ,* Output:
   ,* x(0)
   ,* x(1)
   ,*/

  std::cout << 1 + 2 * x[0] + 3 * x[1] + 4 * x[1] << "\n";
  /**
   ,* Output:
   ,* (1 + 2 * x(0) + 7 * x(1))
   ,*/
#+end_src

To create an array ~y~ of two variables named "dog(0)"" and "dog(1)", pass the name "dog" as a second argument to ~NewContinuousVariables()~.
Also shown below is the printout of the two variables in ~y~ and a symbolic expression involving ~y~.

#+begin_src cpp
  auto y = prog.NewContinuousVariables(2, "dog");
  std::cout << y << "\n";
  /**
   ,* Output:
   ,* dog(0)
   ,* dog(1)
   ,*/

  std::cout << y[0] + y[0] + y[1] * y[1] * y[1] << "\n";
  /**
   ,* Output:
   ,* (2 * dog(0) + pow(dog(1), 3))
   ,*/
#+end_src

To create a $3 \times 2$ matrix of variables named "A", type

#+begin_src cpp
  auto var_matrix = prog.NewContinuousVariables(3, 2, "A");
  std::cout << var_matrix << "\n";
  /**
   ,* Output:
   ,* A(0,0) A(0,1)
   ,* A(1,0) A(1,1)
   ,* A(2,0) A(2,1)
   ,*/
#+end_src


** Adding constraints
There are many ways to impose constraints on the decision variables. This tutorial shows a few simple examples.
Refer to the links at the [[*Advanced tutorials][bottom]] of this document for other types of constraints.

*** AddConstraint
The simplest way to add a constraint is with ~MathematicalProgram.AddConstraint()~.
#+begin_src cpp
  // Add an equality constraint
  prog.AddConstraint(x[0] * x[1] == 1)
#+end_src


You can also add inequality constraints to `prog` such as
#+begin_src cpp
  // Add inequality constraints
  prog.AddConstraint(x[0] >= 0);
  prog.AddConstraint(x[0] - x[1] <= 0);
#+end_src
** Adding Cost functions
In a complicated optimization problem, it is often convenient to write the total cost function $f(x)$ as a sum of individual cost functions

$\begin{aligned}
f(x) = \sum_i g_i(x)
\end{aligned}$

*** AddCost method
The simplest way to add an individual cost function $g_i(x)$ to the total cost function $f(x)$ is with the ~MathematicalProgram.AddCost()~ method (as shown below).
#+begin_src cpp
  // Add a cost x(0)**2 + 3 to the total cost. Since prog doesn't have a cost before, now the total cost is x(0)**2 + 3
  prog.AddCost(x[0] ** 2 + 3);
#+end_src

To add another individual cost function $x(0) + x(1)$ to the total cost function $f(x)$, simply call ~AddCost()~ again as follows
#+begin_src cpp
  prog.AddCost(x[0] + x[1]);
#+end_src

now the total cost function becomes $x(0)^2 + x(0) + x(1) + 3$.

~prog~ can analyze each of these individual cost functions and determine that $x(0) ^ 2 + 3$  is a convex quadratic function, and $x(0) + x(1)$ is a linear function of $x$.

** Solve the optimization problem
Once all the decision variables/constraints/costs are added to ~prog~, we are ready to solve the optimization problem.
*** Automatically choosing a solver
The simplest way to solve the optimization problem is to call ~Solve()~ function. Drake's MathematicalProgram analyzes the type of the constraints/costs,
and then calls an appropriate solver for your problem. The result of calling `Solve()` is stored inside the return argument. Here is a code snippet
#+begin_src cpp
  #include <drake/solvers/mathematical_program.h>
  #include <drake/solvers/solve.h>

  #include <iostream>

  template <typename... Args>
  void print(Args&&... value) {
    (std::cout << ... << value) << "\n---\n";
  }

  int main(int argc, char* argv[]) {
    /*
     ,* Solves a simple optimization problem
     ,*    min x(0)^2 + x(1)^2
     ,* subject to x(0) + x(1) = 1
     ,*        x(0) <= x(1)
     ,*/
    // Set up the optimization problem.
    auto prog = drake::solvers::MathematicalProgram();
    auto x = prog.NewContinuousVariables(2);
    prog.AddConstraint(x[0] + x[1] == 1);
    prog.AddConstraint(x[0] <= x[1]);
    prog.AddCost(pow(x[0], 2) + pow(x[1], 2));

    // Now solve the optimization problem.
    auto result = drake::solvers::Solve(prog);

    // print out the result.
    print("Success? ", result.is_success());
    // Print the solution to the decision variables.
    print("x* = ", result.GetSolution(x));
    // Print the optimal cost.
    print("optimal cost = ", result.get_optimal_cost());
    // Print the name of the solver that was called.
    print("solver is: ", result.get_solver_id().name());
    return 0;
  }
#+end_src

Notice that we can then retrieve optimization result from the return argument of ~Solve~. For example, the solution $x^*$ is retrieved from ~result.GetSolution()~,
and the optimal cost from ~result.get_optimal_cost()~. To play around with it, [[https://github.com/Maverobot/docker-compiler-explorer-drake#usage][launch compiler-explorer locally]] and click [[http://localhost:10240/#z:OYLghAFBqd5QCxAYwPYBMCmBRdBLAF1QCcAaPECAMzwBtMA7AQwFtMQByARg9KtQYEAysib0QXACx8BBAKoBnTAAUAHpwAMvAFYTStJg1DIApACYAQuYukl9ZATwDKjdAGFUtAK4sGIMwAcpK4AMngMmAByPgBGmMQS0gAOqAqETgwe3r7%2BQSlpjgJhEdEscQlStpj2hQxCBEzEBFk%2BfoFVNRn1jQTFUbHxibYNTS057QojveH9ZYNSAJS2qF7EyOwc5gDM4cjeWADUJltu6MRMANaYAPQKngBu8QrXLEwECJivjqK0APpJxFQwHOLAAdAhjtgTBoAILbXb7TBHE5nS43O60R7EZ4Yx7gyHQuGw%2BEMPZeQ7HNxOSbETCsAnE2EET5JAzM5FuAgATySjFYmFBgoOMOIwAUDJh91QeHQBwB4QIEBFYvMADY1YLQQd7mIvJgFkcAOxWWEHA4QSboEAoFYEDmUg6a%2B0nbW6/XOtxHMxmEwAVjcDAAtMG/QHzD6tia4YaACKEwkKg6vcIQRONYDIUgHZAIRoAKgO6fufqsvpjBpMxsJZuueerZoLQgemAUhYOaRYrKRqCSjhYeAAXm8MnLATF6Cx6wcC2ak%2BEDqoIBoFn7sGYvRYFxAuCvfWupwWFF4YtpMA4DkQt8uN1ud8iYwcuAfZy%2BrxWTscH4udwfrvXrtcBxCJgdpeEkF4fAcPZ9oOw4CKOqDjp8oL1kwXiXgCQL3gcqJXNauJPNaACybwfF8eA/MogLAqwEArpGqHoagC7YZhwCgpEmAAO4eII4ReCsCgAGqNHgTBIQoEBmPRUZmvKggQOGqjYeGWaqDJjGXmgDA0kwCpcKx1GgjC6DuAIukKhAqglhofoPtYC4lk%2BZb3p%2Bj4aaao6WeG2kWYIXAgF6ZhZr5BDnPpHkwmaaFaeZYV6YI65uWxxmmbxfmKtZvoWLZLmUm5WUWM55bHLJXkKT5cXhYlgWqdmVUJQQ0mlZpzFoJMBnJUZJlmZMEApFxVk2XZWbSTeA1DdlxWjQskVycQ3neu1BABUFIWpCtkX/oBkSoFx7bNhB3a9ng/ZDrUCFIWCrUHLSR60Habm4ewIAEdi1pNpimD9dRW2eQB5V2raR23S2XgPShnnyYq4ZCF4yDrAoCi1d6WZ3eDBCgngCi/EeCMtpJs0tf9gFUYm7xIhi6EjpeFM4We2MjjqC3ifQCiQ1FgOKd6qgFm5dXoxDADiIGfdTAhWUTDEkwcZOCCD0GnWI9WTBz82LWYiuvLQKuPVs9mo6D92Y8AIG/FrYi/MtdFzQcANy3adPMGwUFUCDb0QW8BxcUwrY/PQ6Bq1z4Ye9jKPBUbGOgqbBC482xC/DKdGgs731S2VtIEKsDAHLZ0vRnGsIcEstCcL6vB%2BBwWikKgnBuNYDl3Ks6xelsPCkAQmjF0sFwgL6kighoqpbAAnIakgBGYI%2B%2BoavpmJI0ilxwkgV13NecLwyMaB3XdLHAsBIGgnZ0PE5CUEfSQnwkwAj3wdDMu9EAxGvMThI0XKcO3R9sIIADyDC0A/lXXgWBXhGHEMA0g%2BBaQODwI8ZGkDMCqDPOhDY1cFTVDXrQPAMRzjEC5B4LAa8wqnU/sXPgBgxRCTwNxX%2BvJK7t34IIEQYh2CVCYfIJQag166C4PoQwxgG6WH0Dg5GsBU4gEwHgU2DBSBYk4FsLYhpyEQAkc9ORgxuBcC0LNGuJ1zKcEDJaT8phLDWC4MojoZ5aguAYGZbIfg%2BGhBmKUcoeh8jpAEGMRxpAPG1D6K4%2BYVjYECG6KMTwrQ9B2GsV0KYASBgVGGD0bxUS4kuISRIJYTc1hsI7rSDYPAS5l1XpA2uHBgBCM3CPc09czHCIOLgQgJBW58IOB4Y%2B9BiAtIWLwTuwC95IFtEkdCZ8IAXyvnoTA%2BAiCJI4Sw8Q7DZCKBUOoSBYVMAFN4Fxc4SQyFFI4OXUgldq5lN/uhYZQM3YVLqVUmpqAOnxG6b03ePcQCSF9KCEeWxVQaDML6CxGhJDz1nqqfQnAV6kBYBIDQ29jm8DKZvPQfSdH7LMCUk5G8d79KWFiNIzhJBAA%3D][here]].

Some optimization solution is infeasible (doesn't have a solution). For example in the following code example, ~result.get_solution_result()~ will not report ~kSolutionFound~.

#+begin_src cpp
  /*
   ,* An infeasible optimization problem.
   ,*/
  auto prog = MathematicalProgram();
  auto x = prog.NewContinuousVariables(1)[0];
  auto y = prog.NewContinuousVariables(1)[0];
  prog.AddConstraint(x + y >= 1);
  prog.AddConstraint(x + y <= 0);
  prog.AddCost(x);

  auto result = Solve(prog);
  print("Success? ", result.is_success());
  print(result.get_solution_result());
#+end_src

To play around with it, [[https://github.com/Maverobot/docker-compiler-explorer-drake#usage][launch compiler-explorer locally]] and click [[http://localhost:10240/#g:!((g:!((g:!((h:codeEditor,i:(filename:'1',fontScale:14,fontUsePx:'0',j:1,lang:c%2B%2B,selection:(endColumn:1,endLineNumber:29,positionColumn:1,positionLineNumber:29,selectionStartColumn:1,selectionStartLineNumber:29,startColumn:1,startLineNumber:29),source:'%23include+%3Cdrake/solvers/mathematical_program.h%3E%0A%23include+%3Cdrake/solvers/solve.h%3E%0A%0A%23include+%3Ciostream%3E%0A%0Atemplate+%3Ctypename...+Args%3E%0Avoid+print(Args%26%26...+value)+%7B%0A++(std::cout+%3C%3C+...+%3C%3C+value)+%3C%3C+%22%5Cn---%5Cn%22%3B%0A%7D%0A%0Aint+main(int+argc,+char*+argv%5B%5D)+%7B%0A++auto+prog+%3D+drake::solvers::MathematicalProgram()%3B%0A++auto+x+%3D+prog.NewContinuousVariables(1)%5B0%5D%3B%0A++print(%22x+%3D+%22,+x)%3B%0A++auto+y+%3D+prog.NewContinuousVariables(1)%5B0%5D%3B%0A++print(%22y+%3D+%22,+y)%3B%0A++auto+constraint1+%3D+prog.AddConstraint(x+%2B+y+%3E%3D+1)%3B%0A++print(%22constraint1:+%22,+constraint1)%3B%0A++auto+constraint2+%3D+prog.AddConstraint(x+%2B+y+%3C%3D+0)%3B%0A++print(%22constraint2:+%22,+constraint2)%3B%0A++auto+cost1+%3D+prog.AddCost(x)%3B%0A++print(%22cost1:+%22,+cost1)%3B%0A%0A++auto+result+%3D+drake::solvers::Solve(prog)%3B%0A++print(%22Success:+%22,+result.is_success())%3B%0A++print(%22Solution+result:+%22,+result.get_solution_result())%3B%0A++return+0%3B%0A%7D%0A'),l:'5',n:'0',o:'C%2B%2B+source+%231',t:'0')),k:54.063974829575244,l:'4',n:'0',o:'',s:0,t:'0'),(g:!((h:compiler,i:(compiler:g9,filters:(b:'0',binary:'1',commentOnly:'0',demangle:'0',directives:'0',execute:'0',intel:'0',libraryCode:'0',trim:'1'),flagsViewOpen:'1',fontScale:14,fontUsePx:'0',j:1,lang:c%2B%2B,libs:!((name:eigen,ver:'337'),(name:drake,ver:'110')),options:'-std%3Dc%2B%2B17',selection:(endColumn:1,endLineNumber:1,positionColumn:1,positionLineNumber:1,selectionStartColumn:1,selectionStartLineNumber:1,startColumn:1,startLineNumber:1),source:1,tree:'1'),l:'5',n:'0',o:'g%2B%2B+9+(C%2B%2B,+Editor+%231,+Compiler+%231)',t:'0'),(h:output,i:(compiler:1,editor:1,fontScale:14,fontUsePx:'0',tree:'1',wrap:'1'),l:'5',n:'0',o:'Output+of+g%2B%2B+9+(Compiler+%231)',t:'0')),k:45.936025170424756,l:'4',m:100,n:'0',o:'',s:1,t:'0')),l:'2',n:'0',o:'',t:'0')),version:4][here]].

*** Manually choosing a solver

If you want to choose a solver yourself, rather than Drake choosing one for you, you could instantiate a solver explicitly, and call its =Solve=
function. There are two apporaches to instantiate a solver. For example, if I want to solve a problem using the open-source solver [[https://github.com/coin-or/Ipopt][IPOPT]],
I can instantiate the solver using either of the two approaches:
1. The simplest approach is to call
   #+begin_src cpp
     #include <drake/solvers/ipopt_solver.h>
     // ...
     auto solver = drake::solvers::IpoptSolver();
   #+end_src
2. The second approach is to construct a solver with a given solver ID as
   #+begin_src cpp
     #include <drake/solvers/choose_best_solver.h>
     // ...
     auto solver = drake::solvers::MakeSolver(drake::solvers::IpoptSolver::id());
   #+end_src

A complete example:
#+begin_src cpp
  #include <drake/solvers/ipopt_solver.h>
  #include <drake/solvers/mathematical_program.h>

  #include <iostream>

  template <typename... Args>
  void print(Args&&... value) {
    (std::cout << ... << value) << "\n---\n";
  }

  int main(int argc, char* argv[]) {
    /*
     ,* Solves a simple optimization problem
     ,*
     ,* min x(0)
     ,*
     ,* subject to x(0) + x(1) = 1
     ,*            0 <= x(1) <= 1
     ,*/
    // Set up the optimization problem.
    auto prog = drake::solvers::MathematicalProgram();
    auto x = prog.NewContinuousVariables(2);
    print("x = ", x);
    auto constraint1 = prog.AddConstraint(x[0] + x[1] == 1);
    print("constraint1: ", constraint1);
    auto constraint2 = prog.AddConstraint(0 <= x[1]);
    print("constraint2: ", constraint2);
    auto constraint3 = prog.AddConstraint(x[1] <= 1);
    print("constraint3: ", constraint3);
    auto cost1 = prog.AddCost(x[0]);
    print("cost1: ", cost1);

    drake::solvers::IpoptSolver solver;
    // The initial guess is [1, 1]. The third argument is the options for Ipopt solver, and we set no
    // solver options.
    Eigen::VectorXd initial_guess(2);
    initial_guess << 1, 1;
    auto result = solver.Solve(prog, initial_guess, std::nullopt);
    print(result.get_solution_result());
    print("x* = ", result.GetSolution(x));
    print("Solver is ", result.get_solver_id().name());
    print("Ipopt solver status: ", result.get_solver_details<drake::solvers::IpoptSolver>().status,
          ", meaning ",
          result.get_solver_details<drake::solvers::IpoptSolver>().ConvertStatusToString());
    return 0;
    ;
  }
#+end_src

To play around with it, [[https://github.com/Maverobot/docker-compiler-explorer-drake#usage][launch compiler-explorer locally]] and click [[http://localhost:10240/#z:OYLghAFBqd5QCxAYwPYBMCmBRdBLAF1QCcAaPECAMzwBtMA7AQwFtMQByARg9KtQYEAysib0QXACx8BBAKoBnTAAUAHpwAMvAFYTStJg1DIApACYAQuYukl9ZATwDKjdAGFUtAK4sGISVykrgAyeAyYAHI%2BAEaYxP4apAAOqAqETgwe3r7%2BgSlpjgKh4VEssfGSiXaYDhlCBEzEBFk%2BfgG2mPaFDPWNBMWRMXEJtg1NLTntCmP9YYNlw5UAlLaoXsTI7BzmAMxhyN5YANQmO27oxEwA1pgA9AqeAG5xCrd4KUkEAPoPtM/EADoEKdsCYNABBXb7Q6YE5nC7XO6/f6vFhMAgITBoxyiWhfJLEVDAS4sIEgsGQiFQhgHLzHU5uJzTYiYVjkqkQghYpIGLlwtwEACeSUYrEwAIlR3BxGACnZ4MeqDw6COBLCBAg0tl5gAbLqJQCjo8xF5MEsTgB2KwQo5HCDTdAgFBrAj8hlHA1us5Gk1mr1uE5mMwmACsbgYAFoo6Hw%2BZgztrZCLQARCkU9VHNFhCAZxrAZCkI7IBCNABURzzj1DVhDyfNJitFNtt1LTdt5aET0wCgrRzSLB5sNQnzwLDwAC90RlVYTovQWG2jq2be3M2EjqoIBolovl%2BDbUu%2B15otoaq6iBut/XLJeuPWdsmjlxdwfX2%2BNF7H5u75%2Bn7vbm2ty3EcQiYK6XhJEcGJDiOY6Tt0M6oHOWIAm2TBeBeBJEnCj4IjcTrIi8ToALLopi2J4LiyiEsSrAQDuCZoRhqAbjhiHAACESYAA7h4ghhF4awKAAao0eBMMhCgQGYDGJraaqCBAcaqGxcaFqoslMReaAMMyTDqlwbFYRx4LoO4Ah6eqECqNWGiho%2B1gbtWz61jhpyPnepxyTOVlxjplmCFwICBmYhb%2BQQlwGZpK7odpFkRfpghmEZNEAqZ5m6QlVkfgy7lOSGFguXWXltgpGp%2BfFkVJcFalFpViUEDJJUxcxdWZVVBA7ClRJpWZfEBRqNkFUVv6eYxK5lUpQbhR1Ow1UGYX1eqOzRfuFatWg0yGXlxm9eZ0zWbZ9mrfJxC%2BdNqQEEFIWLVtq1tnh7AgIRxAKE6ACSHzCF2xB9j9zVrUBRwACqYkcYTpGIRzAKaCg9ngPbOYWRWGqDsIYngxAqnmPiMK6CNQWDw7dD2/C/Z9xN/X8cSFoYKrcbCSiugwqCAcBL1HMTGQKKhK7YHgwCME6wlniQAAaKoQ44YhfDD3ZSU141rVL4l4nLcP%2Bk%2ByMA7asUsSyCheLQrp5S9AKdtTEDGYWKsy%2BrCiFg6ToMEbtDEydPmKQbRsEACgvfL8GEZF83vG/RHuTcp5Z5bVoe%2BwA4mBFtBwI1lLBHZ2KXGFv/ODiMLUccd%2B2BPw/V8yr0QCzBsOHOue%2BVQYU58VO59M6JeG9N2F92PvFwHZdYA0dByvClz4c9P1vSATffdTxAgpXbcEB3pCLm%2BtVsIYYTADda%2BvkX/ul3PXyD/ptAj%2BcY9PS9U8zzncQL0sAJ8f8wgNMvCjA6g9SZ8AtdK7aFky9iAMCOHZABcJEwNlTBCDgKxaCcBDLwPwHAtCkFQJwNw1hHIPHWJsQMOweCkAIJoOBKwrggBDJIAEGgdQ7AAJwWkkAADjMPQkMFoQxmEkNIBBHBJDINIegzgvA3qJBIaguBpA4CwCQGgAcdAaYUAgPIpIij4jAHoXwOgXJXqUGiEI6IYRGiCk4EQ%2BRbBBAAHkGC0FMZI0gWA0RGHEA4/ALJajPDeg4zAqgagYS2Gg9UnQhG0DwNES4xBBQeCwEIiKo4zFSKoAYWUwk8A8SsSKFBRD%2BCCBEGIdgUgZCCEUCodQDjdCBAMEYFA2DLD6HCW9WA1d2CYAFowUg/xOA7B2BaKREAWkgEep04Y3AuBaHTugkcFlOARgdO5UwlhrBcD6R0LoGQXAMHMtkPwgQQhzFKOUPQ%2BR0gCAmLs5IqRTkMAGIc4YgRqi1AEL0cYnhWh6Eed0F5swShDHiA8mY5yPkzFuX8iQKxcEbEKcQlkWweDwMQYIhxGCODADqRYI49C7RYKWfUo4uBCAkAIYEI4HgFH0F%2BrsO8vAJETOkUgF0SQMLkEoKo9RehMD4CIP84pwhcSFOkLk%2BQSg1BxNhYk0g3FLhJESQijgSDSAoLQSiqxGEmWulQFQaG6LMXYtQOSuIxKlg0tIZMihkgQwAnoTsHUGgzAhhWRoSQ3DOE6n0JwARpAWASA0IkJVvAUWiL0LSsh7qOBmCRcqkRxDTUrBRBsyQQA][here]].

Note that =solver.Solve()= expects three input arguments, the optimization program =prog=, the initial guess of the decision variable
values (=[1, 1]= in this case), and an optional setting for the solver (=None= in this case, we use the default IPOPT setting). If you don't
have an initial guess, you could call =solver.Solve(prog)=. Drake will choose a default initial guess (a 0-valued vector), but this initial
guess might be a bad starting point for optimization. Note from the following example code, with the default initial guess, the solver
cannot find a solution, even though a solution exists (and could be found with initial guess [1, 1]).

#+begin_src cpp
  auto result = solver.Solve(prog);
  print(result.get_solution_result());
  print("x* = ", result.GetSolution(x));
  /*
   ,* Output:
   ,* SolutionFound
   ,* ---
   ,* x* = -9.99e-09
   ,*         1
   ,*/
#+end_src

Also note that if we know which solver is called, then we can access some solver-specific result, by calling =result.get_solver_details()=.
For example, =IpoptSolverDetails= contains a field =status=, namely the status code of the IPOPT solver, we could access this info by

#+begin_src cpp
  print("Ipopt solver status: ", result.get_solver_details<drake::solvers::IpoptSolver>().status,
        ", meaning ",
        result.get_solver_details<drake::solvers::IpoptSolver>().ConvertStatusToString());
#+end_src

Each solver has its own details. You should refer to =FooSolverDetails= class on what is stored inside the return argument of
=result.get_solver_details()=. For example, if you know that IPOPT is called, then refer to =IpoptSolverDetails= class; for OSQP solver, refer
to =OsqpSolverDetails=, etc.

** Using an initial guess

Some optimization problems, such as nonlinear optimization, require an initial guess. Other types of problems, such as quadratic programming,
mixed-integer optimization, etc, can be solved faster if a good initial guess is provided. The user could provide an initial guess as an input
argument in =Solve= function. If no initial guess is provided, Drake will use a zero-valued vector as the initial guess.

In the example below, we show that an initial guess could affect the result of the problem. Without an user-provided initial guess, the
solver might be unable to find the solution.

#+begin_src cpp
  #include <drake/solvers/ipopt_solver.h>
  #include <drake/solvers/mathematical_program.h>

  #include <iostream>

  template <typename... Args>
  void print(Args&&... value) {
    (std::cout << ... << value) << "\n---\n";
  }

  int main(int argc, char* argv[]) {
    // Set up the optimization problem.
    auto prog = drake::solvers::MathematicalProgram();
    auto x = prog.NewContinuousVariables(2);
    prog.AddConstraint(pow(x[0], 2) + pow(x[1], 2) == 100.);
    prog.AddCost(pow(x[0], 2) - pow(x[1], 2));
    auto solver = drake::solvers::IpoptSolver();
    // The user doesn't provide an initial guess.
    auto result = solver.Solve(prog, std::nullopt, std::nullopt);
    print("Without a good initial guess, success? ", result.is_success());
    print("Solution:", result.GetSolution(x));
    // Pass an initial guess
    Eigen::VectorXd initial_guess(2);
    initial_guess << -5, 0;
    result = solver.Solve(prog, initial_guess, std::nullopt);
    print("With a good initial guess, success? ", result.is_success());
    print("Solution:", result.GetSolution(x));
    return 0;
    ;
  }
#+end_src

To play around with it, [[https://github.com/Maverobot/docker-compiler-explorer-drake#usage][launch compiler-explorer locally]] and click [[http://localhost:10240/#g:!((g:!((g:!((h:codeEditor,i:(filename:'1',fontScale:14,fontUsePx:'0',j:1,lang:c%2B%2B,selection:(endColumn:1,endLineNumber:31,positionColumn:1,positionLineNumber:31,selectionStartColumn:1,selectionStartLineNumber:31,startColumn:1,startLineNumber:31),source:'%23include+%3Cdrake/solvers/ipopt_solver.h%3E%0A%23include+%3Cdrake/solvers/mathematical_program.h%3E%0A%0A%23include+%3Ciostream%3E%0A%0Atemplate+%3Ctypename...+Args%3E%0Avoid+print(Args%26%26...+value)+%7B%0A++(std::cout+%3C%3C+...+%3C%3C+value)+%3C%3C+%22%5Cn---%5Cn%22%3B%0A%7D%0A%0Aint+main(int+argc,+char*+argv%5B%5D)+%7B%0A++//+Set+up+the+optimization+problem.%0A++auto+prog+%3D+drake::solvers::MathematicalProgram()%3B%0A++auto+x+%3D+prog.NewContinuousVariables(2)%3B%0A++prog.AddConstraint(pow(x%5B0%5D,+2)+%2B+pow(x%5B1%5D,+2)+%3D%3D+100.)%3B%0A++prog.AddCost(pow(x%5B0%5D,+2)+-+pow(x%5B1%5D,+2))%3B%0A++auto+solver+%3D+drake::solvers::IpoptSolver()%3B%0A++//+The+user+doesn!'t+provide+an+initial+guess.%0A++auto+result+%3D+solver.Solve(prog,+std::nullopt,+std::nullopt)%3B%0A++print(%22Without+a+good+initial+guess,+success%3F+%22,+result.is_success())%3B%0A++print(%22Solution:%22,+result.GetSolution(x))%3B%0A++//+Pass+an+initial+guess%0A++Eigen::VectorXd+initial_guess(2)%3B%0A++initial_guess+%3C%3C+-5,+0%3B%0A++result+%3D+solver.Solve(prog,+initial_guess,+std::nullopt)%3B%0A++print(%22With+a+good+initial+guess,+success%3F+%22,+result.is_success())%3B%0A++print(%22Solution:%22,+result.GetSolution(x))%3B%0A++return+0%3B%0A++%3B%0A%7D%0A'),l:'5',n:'0',o:'C%2B%2B+source+%231',t:'0')),k:54.063974829575244,l:'4',n:'0',o:'',s:0,t:'0'),(g:!((h:compiler,i:(compiler:g9,filters:(b:'0',binary:'1',commentOnly:'0',demangle:'0',directives:'0',execute:'0',intel:'0',libraryCode:'0',trim:'1'),flagsViewOpen:'1',fontScale:14,fontUsePx:'0',j:1,lang:c%2B%2B,libs:!((name:eigen,ver:'337'),(name:drake,ver:'110')),options:'-std%3Dc%2B%2B17',selection:(endColumn:1,endLineNumber:1,positionColumn:1,positionLineNumber:1,selectionStartColumn:1,selectionStartLineNumber:1,startColumn:1,startLineNumber:1),source:1,tree:'1'),l:'5',n:'0',o:'g%2B%2B+9+(C%2B%2B,+Editor+%231,+Compiler+%231)',t:'0'),(h:output,i:(compiler:1,editor:1,fontScale:14,fontUsePx:'0',tree:'1',wrap:'1'),l:'5',n:'0',o:'Output+of+g%2B%2B+9+(Compiler+%231)',t:'0')),k:45.936025170424756,l:'4',m:100,n:'0',o:'',s:1,t:'0')),l:'2',n:'0',o:'',t:'0')),version:4][here]].

For more details on setting the initial guess, the user could refer to [[./nonlinear_program.ipynb][Nonlinear program]] section =Setting the initial guess=.

* Add callback

Some solvers support adding a callback function in each iteration. One usage of the callback is to visualize the solver progress in the current
iteration. =MathematicalProgram= supports this usage through the function =AddVisualizationCallback=, although the usage is not limited
to just visualization, the callback function can do anything. Here is an example

#+begin_src cpp
  #include <drake/solvers/mathematical_program.h>
  #include <drake/solvers/solve.h>

  #include <iostream>

  template <typename... Args>
  void print(Args&&... value) {
    (std::cout << ... << value) << "\n---\n";
  }

  void update(const Eigen::Ref<const Eigen::VectorXd>& x) {
    print("x = ", x.transpose());
  }

  int main(int argc, char* argv[]) {
    auto prog = drake::solvers::MathematicalProgram();
    auto x = prog.NewContinuousVariables(2);
    prog.AddConstraint(x[0] * x[1] == 9);
    prog.AddCost(pow(x[0], 2) + pow(x[1], 2));
    prog.AddVisualizationCallback(update, x);

    Eigen::VectorXd x_init(2);
    x_init << 4, 5;
    drake::solvers::Solve(prog, x_init);
    return 0;
  }
#+end_src

To play around with it, [[https://github.com/Maverobot/docker-compiler-explorer-drake#usage][launch compiler-explorer locally]] and click [[http://localhost:10240/#g:!((g:!((g:!((h:codeEditor,i:(filename:'1',fontScale:14,fontUsePx:'0',j:1,lang:c%2B%2B,selection:(endColumn:1,endLineNumber:27,positionColumn:1,positionLineNumber:27,selectionStartColumn:1,selectionStartLineNumber:27,startColumn:1,startLineNumber:27),source:'++%23include+%3Cdrake/solvers/mathematical_program.h%3E%0A++%23include+%3Cdrake/solvers/solve.h%3E%0A%0A++%23include+%3Ciostream%3E%0A%0A++template+%3Ctypename...+Args%3E%0A++void+print(Args%26%26...+value)+%7B%0A++++(std::cout+%3C%3C+...+%3C%3C+value)+%3C%3C+%22%5Cn---%5Cn%22%3B%0A++%7D%0A%0A++void+update(const+Eigen::Ref%3Cconst+Eigen::VectorXd%3E%26+x)+%7B%0A++++print(%22x+%3D+%22,+x.transpose())%3B%0A++%7D%0A%0A++int+main(int+argc,+char*+argv%5B%5D)+%7B%0A++++auto+prog+%3D+drake::solvers::MathematicalProgram()%3B%0A++++auto+x+%3D+prog.NewContinuousVariables(2)%3B%0A++++prog.AddConstraint(x%5B0%5D+*+x%5B1%5D+%3D%3D+9)%3B%0A++++prog.AddCost(pow(x%5B0%5D,+2)+%2B+pow(x%5B1%5D,+2))%3B%0A++++prog.AddVisualizationCallback(update,+x)%3B%0A%0A++++Eigen::VectorXd+x_init(2)%3B%0A++++x_init+%3C%3C+4,+5%3B%0A++++drake::solvers::Solve(prog,+x_init)%3B%0A++++return+0%3B%0A++%7D%0A'),l:'5',n:'0',o:'C%2B%2B+source+%231',t:'0')),k:54.063974829575244,l:'4',n:'0',o:'',s:0,t:'0'),(g:!((h:compiler,i:(compiler:g9,filters:(b:'0',binary:'1',commentOnly:'0',demangle:'0',directives:'0',execute:'0',intel:'0',libraryCode:'0',trim:'1'),flagsViewOpen:'1',fontScale:14,fontUsePx:'0',j:1,lang:c%2B%2B,libs:!((name:eigen,ver:'337'),(name:drake,ver:'110')),options:'-std%3Dc%2B%2B17',selection:(endColumn:1,endLineNumber:1,positionColumn:1,positionLineNumber:1,selectionStartColumn:1,selectionStartLineNumber:1,startColumn:1,startLineNumber:1),source:1,tree:'1'),l:'5',n:'0',o:'g%2B%2B+9+(C%2B%2B,+Editor+%231,+Compiler+%231)',t:'0'),(h:output,i:(compiler:1,editor:1,fontScale:14,fontUsePx:'0',tree:'1',wrap:'1'),l:'5',n:'0',o:'Output+of+g%2B%2B+9+(Compiler+%231)',t:'0')),k:45.936025170424756,l:'4',m:100,n:'0',o:'',s:1,t:'0')),l:'2',n:'0',o:'',t:'0')),version:4][here]].

* Advanced tutorials

Setting solver parameters

Updating costs and constraints(e.g. for efficient solving of many similar programs)

Debugging tips

Linear program

Quadratic program

Nonlinear program

Sum-of-squares optimization