Skip to content

Instantly share code, notes, and snippets.

@jwnimmer-tri
Created February 20, 2019 03:29
Show Gist options
  • Save jwnimmer-tri/fedef21baa5d76cd432eb32929ac938e to your computer and use it in GitHub Desktop.
Save jwnimmer-tri/fedef21baa5d76cd432eb32929ac938e to your computer and use it in GitHub Desktop.

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)
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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment