Drake: Refactor MathematicalProgram

[hongkai-dai]

Edited to add: Instructions for porting to the new APIs

If you are a Drake user and would like to adjust your code to the new APIs, please see below for details.

Original issue write-up

We have discussed about refactoring MP in several issues (#8507, #8402 #6929 #5683 #5654 #4559 #4508). @jwnimmer-tri and I just had a discussion during the onsite, and we plan to do the refactoring in this quarter.

Issues

There are several issues in the current MathematicalProgram

1. Circular dependency. MathematicalProgram depends on MathematicalProgramSolverInterface, and vice versa.
2. The target //solvers:mathematical_program has to link against all solvers even if the user only wants to use one.
3. We can't call MathematicalProgram::Solve in multiple threads, since Solve() function is not a const method. MathematicalProgram stores the result of the most recent Solve, and the stored result is modified in the Solve function.
4. The solver-specific result is ignored, such as the active set, the dual variable values, solver computing time, etc.
5. It's hard for external users to add their own solver, as they will have to modify the solver dispatch logic (which solver should be called, given the cost/constraints in MathematicalProgram). This is not a very big issue though.

Proposal

1. MathematicalProgram only contains cost / constraints / solver options. It will not contain the initial guess and the result. This should solve the "constness" issue of Solve function, and thus allow Solve to be called in multiple threads.
2. Create a class MathematicalProgramResult
C++ class MathematicalProgramResult { SolutionResult result; Eigen::VectorXd x_val; double optimal_cost; unique_ptr<AbstractValue> solver_details }
   The field solver_details contains the solver specific details (which solver is called, solver status code, active set, lagrangian multipliers, etc). The function Solve will return MathematicalProgramResult.
3. Break the circular dependency. MathematicalProgram loses Solve function, the solver dispatch and the result. We add a free Solve function, that depends on both MathematicalProgram and MathematicalProgramSolverInterface. The syntax would be
   C++ void Solve(const MathematicalProgram& prog, const optional<VectorXd>& x_initial_guess, MathematicalProgramResult* result);
   Inside the Solve function, it creates the solver by checking the cost/constraints of the program. Since MathematicalProgram doesn't depend on each solvers, this also solves issue 2. If the user wants to use just one solver (say OSQP), then he only need to link against MathematicalProgram and osqp_solver. The users will need to change their call to MathematicalProgram::Solve in the current code.

@RussTedrake @EricCousineau-TRI @avalenzu

[EricCousineau-TRI]

[... ] We can't call MathematicalProgram::Solve in multiple threads, since Solve() function is not a const method. [...]

@avalenzu mentioned today that it'd be nice to have some mechanism to support "caching" of sorts, e.g. being able to share a MutlibodyTreeContext among different constraints such that it's thread safe. The easiest solution specific to that problem is to have the constraint be aware of its execution context through some global mechanism (e.g. querying std::this_thread::get_id() or omp_get_thread_num()).

A more general solution is to somehow pass the relevant execution context through a "session" that constraints can access. However, I'm not sure of the best formulation, as it might require some sort of dynamic typing (e.g. something like std::map<string, AbstractValue>), and would require some allocation constraints (resolving resource conflicts where two constraints have different interpretations of the same registered value).

Context comes to mind, but may be more bulky than necessary (we don't need the value structure it prescribes); also, caching is ticket based, so there may not be much to gain from caching between constraints for the same decision variable values.
(That being said, from looking at the present implementation MultibodyTreeContext, I guess we recompute stuff for the same values anywho.)

Should we open this up as a separate issue, make a design and incorporate it here, or just keep it in mind? (I think the above workaround would remove any immediate blockers)

\cc @calderpg-tri

[jwnimmer-tri]

... e.g. being able to share a MutlibodyTreeContext among different constraints such that it's thread safe.

I understood the request to be different than that. The request is that the user be able to formulate a single MathematicalProgram with some problem that has multibody-related constraints, but then can Solve that program in multiple threads concurrently, from different initial guesses. The challenge now is that the Constraint must embed a pointer to both the MBP and the MBP's Context -- the former can be used const-concurrently from many threads, but the latter is mutable and cannot be. The request is that the user formulating the problem supply the constraints and single MBP, but that during the Solve, each thread uses its own Context.

I'll suggest that the right solution here is that the Context be owned by the Result struct, which then makes it Solve-specific. The framework ask for this is to have a way for Constraint or Cost evaluation to be able to access the Result they are solving with respect to.

[hongkai-dai]

As mentioned in #8647, we need to also handle when the optimization program is empty.

[hongkai-dai]

@jwnimmer-tri and I had started the refactoring process. We would like to list the action items to track the progress

• [x] Add MathematicalProgramResult class
• [x] Add a function
  C++ MathematicalProgramResult = XXXSolver::Solve(const MathematicalProgram& prog, optional<Eigen::VectorXd> x_initial_value);
  for each solver
  
  
  
  • [x] Mosek
  
  
  • [x] Gurobi
  
  
  • [x] OSQP
  
  
  • [x] Snopt
  
  
  • [x] Ipopt
  
  
  • [x] NLopt
  
  
  • [x] SCS
  
  
  • [x] Moby
  
  
  • [x] dReal
  
  
  • [x] LinearSystemSolver
  
  
  • [x] EqualityConstrainedQPSolver
  
  
• [x] Create a free function
  C++ std::unique_ptr<MathematicalProgramSolverInterface> ChooseBestSolver(const MathematicalProgram& prog);
  to choose the best solver given the program formulation.
• [x] Create a SolverOption class. MathematicalProgram stores the solver options.
• [x] Create a free function
  C++ MathematicalProgramResult Solve(const MathematicalProgram& prog, optional<Eigen::VectorXd> x_initial_guess, optional<SolverOption> solver_option);
  Inside this function, it first chooses the best solver, and then calls Solve function of that solver.
• [x] Add pydrake bindings for all of the new APIs.
• [x] Deprecate MathematicalProgram::Solve, remove initial values and decision variable values from MathematicalProgram. Remove class SolverResult.
• [ ] In Doxygen, add a tutorial page on using MathematicalProgram.

@jwnimmer-tri is there anything you would like to add?

[hongkai-dai]

I think we have two issues with the proposed refactorization approach above

1. It is not hard to imagine that when running Solve in multiple threads, we want to set different options for each thread. For example, with snopt solver, we can set scale option to 1 in one thread, and 2 in another thread (as suggested by Elizabeth, this option can affect our solver performance a lot). Hence we should pass SolverOption as an optional argument in Solve, instead of storing the options in MathematicalProgram.
2. Currently in Solve(const MathematicalProgram& prog, optional<Eigen::VectorXd> x_initial_guess, we assume that the initial guess is just for the decision variables, as an Eigen vector of doubles. But it is also possible to pass in more information as the initial guess to warm-start the program. For example, we can also pass the guess for the Lagrangian multipliers, or some indication on "which inequality constraint should be active." These initial guesses can be solver specific (some solvers accept setting lagrangian multipliers, some do not). So I would propose that for each solver type, we declare a class XXXSolverInitialGuess, and change Solve function to the following one
   C++ MathematicalProgramResult Solve(const MathematicalProgram& prog, optional<AbstractValue> initial_guess, optional<SolverOption> solver_option) { std::unique_ptr<MathematicalProgramSolverInterface> solver = ChooseBestSolver(prog); return solver.Solve(prog, initial_guess, solver_option); }
   Inside MathematicalProgramSolverInterface::Solve(...), it converts initial_guess from AbstractValue to the right type, like SnoptSolverInitialGuess.
   The downside of this API is that the user has to convert their initial guess to AbstractValue type, which is not very convenient.
   @jwnimmer-tri , what would you think?

[jwnimmer-tri]

Re: (1). Yes, the user should be able to pass solver options _both_ during program declaration, _and_ to the solve call. That's why we want SolverOptions as its own class.

[jwnimmer-tri]

Re: (2) Discussed offline.

For the generic Solve free function, the optional<Eigen> is good enough. When a particular solver back-end is interested in more input data, and the user knows which back-end they are using for certain, the back-end like SnoptSolver can offer a non-virtual Solve overload that takes in the special data, in a solver-specific way.

If in the further future we need less verbose way to pass this data into the solver, we could imagine passing it as part of a @param[inout] result into the generic void Solve(prog, guess, MathematicalProgramResult* result) call -- so that input hints are passed along with the Result that will be filled in, as a convenience. We can defer this until we need it later, though.

[RussTedrake]

@hongkai-dai -- excited to see all this cleanup! and excited for the promised tutorial. In case it helps, @kmuhlrad and @pangtao22 produced a mini tutorial for mathematical program in pset 1b for the manipulation class this term (see mathematical_program_examples.ipynb -- a jupyter notebook).

[hongkai-dai]

Thanks Russ for pointing to the tutorial, that's very cool!

What I am writing now is a doxygen file, with introductory examples, and sections on adding constraint, specifying options, retrieving solutions, etc. This doxygen file is for C++ users. a jupyter notebook is surely a better format for the python users.

@kmuhlrad and @pangtao22 thanks for creating the tutorial, I like it a lot!

[pangtao22]

Actually Greg and Pete wrote the bulk of this tutorial for 6.832. We only took it and made slight modifications.

[jwnimmer-tri]

@hongkai-dai What should we do about MathematicalProgram::SubstituteSolution? I think it will have to be deprecated, since it uses the solution. Should we add it to MathematicalProgramResult instead?

[hongkai-dai]

@hongkai-dai What should we do about MathematicalProgram::SubstituteSolution? I think it will have to be deprecated, since it uses the solution. Should we add it to MathematicalProgramResult instead?

I agree, I think we will need to remove MP::SubstituteSolution, and add MPResult::SubstituteSolution when we need it. For the moment, I don't think we actually need MPResult::SubstitueSolution.

[jwnimmer-tri]

FYI I've posted #10681 as a CI test, where I mark the old methods deprecated, and see what breaks. We can use its build-error log (or compile it locally) to help confirm what work still remains.

[jwnimmer-tri]

FYI I'm going to work on (finishing) adding the pydrake bindings for the new APIs now (=> #10696).

[jwnimmer-tri]

This post contains instructions to users about how to update their code to the new MathematicalProgram APIs.

For C++ code that called MathematicalProgram::Solve

We have deprecated the method MathematicalProgram::Solve() (non-const), because it stores the results back into MathematicalProgram (not threadsafe) and because it induces a dependency cycle (program-uses-solvers and solvers-use-program).

Code that previously looked like this:

#include "drake/solvers/mathematical_program.h"

using drake::solvers::MathematicalProgram;
using drake::solvers::SolutionResult;
// ...

MathematicalProgram prog;
// ... add costs, constraints, etc ...
SolutionResult solution_result = prog.Solve();
if (solution_result != SolutionResult::kSolutionFound) {
  std::cout << "error: " << solution_result << "\n";
  return false;
}
const VectorXd values = prog.GetSolution(my_variables);

Should now be updated to look like this:

#include "drake/solvers/solve.h"

using drake::solvers::MathematicalProgram;
using drake::solvers::MathematicalProgramResult;
using drake::solvers::SolutionResult;
// ...

MathematicalProgram prog;
// ... add costs, constraints, etc ...
const MathematicalProgramResult result = Solve(prog);
if (!result.is_success()) {
  SolutionResult solution_result = result.get_solution_result();
  std::cout << "error: " << solution_result << "\n";
  return false;
}
const VectorXd values = result.GetSolution(my_variables);

Or perhaps with judicious use of auto like this:

#include "drake/solvers/solve.h"

using drake::solvers::MathematicalProgram;
// ...

MathematicalProgram prog;
// ... add costs, constraints, etc ...
const auto result = Solve(prog);
if (!result.is_success()) {
  std::cout << "error: " << result.get_solution_result() << "\n";
  return false;
}
const VectorXd values = result.GetSolution(my_variables);

In short, methods on MathematicalProgram that return results (GetSolution, GetOptimalCost, etc.) should now be called on the result object instead.

For an example of this kind of change, see the diffs to examples/cubic_polynomial/backward_reachability.cc at https://github.com/RobotLocomotion/drake/commit/6cf37e0f9156dc22b1e81b51b01e4f5c627221dd#diff-36be4ac857b3e721dde4b7ef68241b71.

For C++ code that called SomeSpecificSolver::Solve

Code that previously looked like this:

#include "drake/solvers/osqp_solver.h"

using drake::solvers::MathematicalProgram;
using drake::solvers::OsqpSolver;
using drake::solvers::SolutionResult;
// ...

MathematicalProgram prog;
// ... add costs, constraints, etc ...
OsqpSolver solver;
SolutionResult result = solver.Solve(prog);
if (result != solvers::SolutionResult::kSolutionFound) { return false; }
std::cout << prog.GetSolution(alpha) << "\n";

Should now be updated to look like this:

#include "drake/solvers/osqp_solver.h"

using drake::solvers::MathematicalProgram;
using drake::solvers::OsqpSolver;
// ...

MathematicalProgram prog;
// ... add costs, constraints, etc ...
const auto result = solver.Solve(prog, {}, {});
if (!result.is_success()) { return false; }
std::cout << result.GetSolution(alpha) << "\n";

For an example of this kind of change, see the diffs to DoDifferentialInverseKinematics at https://github.com/RobotLocomotion/drake/pull/10709/commits/e1e50f62cbf65b90fe91f90f4320b8e5bc6dcd5c.

For Python code that called MathematicalProgram::Solve

We have deprecated the method MathematicalProgram.Solve() , which stores the results back into the MathematicalProgram itself.

Code that previously looked like this:

from pydrake.solvers import mathematicalprogram as mp
prog = mp.MathematicalProgram()
alphas = prog.NewContinuousVariables(2, "alpha")
# ...
prog.Solve()
print(prog.GetSolution(alphas))

Should now be updated to look like this:

from pydrake.solvers import mathematicalprogram as mp
prog = mp.MathematicalProgram()
alphas = prog.NewContinuousVariables(2, "alpha")
# ...
result = mp.Solve(prog)
print(result.GetSolution(alphas))

For an example change, see the diffs to test_matrix_variables in #10681 at
https://github.com/RobotLocomotion/drake/pull/10681/files#diff-019a5d155ce17a12333ddbaef57ae008R325.

For Python code that called SomeSpecificSolver::Solve

Code that previously looked like this:

from pydrake.solvers import mathematicalprogram as mp
solver = OsqpSolver()
result = solver.Solve(prog)
self.assertEqual(result, mp.SolutionResult.kSolutionFound)
self.assertTrue(np.allclose(prog.GetSolution(x), x_expected))

Should now be updated to look like this:

from pydrake.solvers import mathematicalprogram as mp
solver = OsqpSolver()
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))

For an example change, see the diffs to osqp_solver_test.py in #10681 at
https://github.com/RobotLocomotion/drake/commit/17a42fa62be8e5747e9eb6a47b2817da93e5874a#diff-d1a91487cee5c0506f552051f957083d.

MultipleShooting, DirectTranscription, DirectCollocation

All of the prior advice in this post applies -- please read above. Additionally, methods related to trajectory optimization such as ReconstructInputTrajectory can no longer read the solution from the this MathematicalProgram which they subclass. Therefore, instead users must pass in their MathematicalProgramResult object to these methods now, using the new 1-argument overloads. The same goes for ReconstructStateTrajectory, etc.

[sadraddini]

Just noticed that if the mathematical program is infeasible, calling result.GetSolution(var) will dump the core in python. It is not a big issue, but worth mentioning.

[EricCousineau-TRI]

@sadraddini I think that should go in a separate issue; let me see if I can repro it. If I can, I'll open it up. Thanks!

[hongkai-dai]

@sadraddini what is the solver you used when you solve that optimization problem? I encountered the core dump issue in python when calling IPOPT (with infeasible problem), but if I switch to another solver, then there is no problem. The core dump is caused by IPOPT source code.

[EricCousineau-TRI]

@hongkai-dai I can reproduce it, and will open a separate issue that focuses on it. Let's move the discussion there?

[sadraddini]

@hongkai-dai > what is the solver you used when you solve that optimization problem? I encountered the core dump issue in python when calling IPOPT (with infeasible problem), but if I switch to another solver, then there is no problem. The core dump is caused by IPOPT source code.

• Both OSQP and Gurobi have this problem.

import numpy as np
import pydrake.solvers.mathematicalprogram as MP
import pydrake.solvers.gurobi as Gurobi_drake
import pydrake.solvers.osqp as OSQP
global gurobi_solver,OSQP_solver
import pydrake.solvers.osqp as OSQP_drake
gurobi_solver=Gurobi_drake.GurobiSolver()
OSQP_solver=OSQP_drake.OsqpSolver()

N=30
n=10
np.random.seed(0)
H=np.random.random((N,n))-0.5
h=np.random.random((N,1))-np.random.random(1)

prog=MP.MathematicalProgram()
x=prog.NewContinuousVariables(n,1,"x")
prog.AddLinearConstraint(A=H,ub=h,lb=-np.inf*np.ones((N,1)),vars=x)
prog.AddLinearConstraint(ub=np.inf*np.ones((N,1)),lb=np.zeros((n,1)),v=x)
prog.AddLinearConstraint(np.greater_equal(h,np.dot(H,x),dtype='object'))
c=np.random.random((n,1))
e=np.dot(c.T,x)[0,0]
prog.AddQuadraticCost(np.eye(n),np.zeros(n),x)
result=OSQP_solver.Solve(prog,None, None)
flag=result.is_success()
print flag
if True:
    x_sol=result.GetSolution(x)
    print x_sol

Whether I use OSP_solver or gurobi_solver, I get:

Academic license - for non-commercial use only
False
Segmentation fault (core dumped)

[tri-ltyyu]

Work scoped by this issue has been completed. Tutorial for MP remains and is captured by #11694.

[hongkai-dai]

@sadraddini I believe your problem should have been addressed by #11707