Scs solver for linear programs (RobotLocomotion#7447)

* Work to add the SCS solver * Add the parsing for bounding box constraint * Have some segfault when constructing the sparse matrix, and setting the scs problem data. * Fix the segfault. Now we can handle linear constraint, linear equality constraint and bounding box constraint * Put setting the problem data into one function. * Add SCS capability into MathematicalProgram * Move SCS down on the solver stack. Prefer to use SCS if other more accurate solvers are not available. * fix a typo * minor fix * Add SCS to solver table. * Address Andres' comments * Address Sammy's comments * Address Sammy's comment.
jcmadsen · Nov 17, 2017 · 5f469e4 · 5f469e4
1 parent b349a7d
commit 5f469e4
Show file tree

Hide file tree

Showing 13 changed files with 599 additions and 18 deletions.
diff --git a/drake/solvers/BUILD.bazel b/drake/solvers/BUILD.bazel
@@ -160,6 +160,7 @@ drake_cc_library(
         ":moby_lcp_solver",
         ":mosek_solver",
         ":nlopt_solver",
+        ":scs_solver",
         ":snopt_solver",
         ":solver_id",
         ":solver_type",
@@ -575,16 +576,28 @@ drake_cc_library(
 
 drake_cc_library(
     name = "scs_solver",
-    srcs = [
-        "no_scs.cc",
-        "scs_solver_common.cc",
-    ],
+    srcs = select({
+        "//conditions:default": [
+            "scs_solver.cc",
+            "scs_solver_common.cc",
+        ],
+        "//tools:no_scs": [
+            "no_scs.cc",
+            "scs_solver_common.cc",
+        ],
+    }),
     hdrs = ["scs_solver.h"],
-    deps = [
-        ":mathematical_program_api",
-        "//drake/common:essential",
-        "@scs//:scsdir",
-    ],
+    deps = select({
+        "//conditions:default": [
+            ":mathematical_program_api",
+            "//drake/common:essential",
+            "//drake/math:eigen_sparse_triplet",
+            "@scs//:scsdir",
+        ],
+        "//tools:no_scs": [
+            ":mathematical_program_api",
+        ],
+    }),
 )
 
 # This library is empty unless Gurobi is available.
@@ -943,6 +956,19 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "scs_solver_test",
+    srcs = [
+        "test/scs_solver_test.cc",
+    ],
+    deps = [
+        ":linear_program_examples",
+        ":mathematical_program",
+        ":mathematical_program_test_util",
+        ":quadratic_program_examples",
+    ],
+)
+
 drake_cc_googletest(
     name = "snopt_solver_test",
     srcs = [

diff --git a/drake/solvers/mathematical_program.cc b/drake/solvers/mathematical_program.cc
@@ -23,6 +23,7 @@
 #include "drake/solvers/moby_lcp_solver.h"
 #include "drake/solvers/mosek_solver.h"
 #include "drake/solvers/nlopt_solver.h"
+#include "drake/solvers/scs_solver.h"
 #include "drake/solvers/snopt_solver.h"
 #include "drake/solvers/symbolic_extraction.h"
 
@@ -83,6 +84,10 @@ AttributesSet kMosekCapabilities =
      kRotatedLorentzConeConstraint | kLinearCost | kQuadraticCost |
      kPositiveSemidefiniteConstraint | kBinaryVariable);
 
+// Scs solver capatilities.
+AttributesSet kScsCapabilities =
+    (kLinearEqualityConstraint | kLinearConstraint | kLinearCost);
+
 // Solvers for generic systems of constraints and costs.
 AttributesSet kGenericSolverCapabilities =
     (kGenericCost | kGenericConstraint | kQuadraticCost | kQuadraticConstraint |
@@ -118,7 +123,8 @@ MathematicalProgram::MathematicalProgram()
       linear_system_solver_(new LinearSystemSolver()),
       equality_constrained_qp_solver_(new EqualityConstrainedQPSolver()),
       gurobi_solver_(new GurobiSolver()),
-      mosek_solver_(new MosekSolver()) {}
+      mosek_solver_(new MosekSolver()),
+      scs_solver_(new ScsSolver()) {}
 
 MatrixXDecisionVariable MathematicalProgram::NewVariables(
     VarType type, int rows, int cols, bool is_symmetric,
@@ -653,6 +659,12 @@ SolutionResult MathematicalProgram::Solve() {
   } else if (is_satisfied(required_capabilities_, kGenericSolverCapabilities) &&
              nlopt_solver_->available()) {
     return nlopt_solver_->Solve(*this);
+  } else if (is_satisfied(required_capabilities_, kScsCapabilities) &&
+      scs_solver_->available()) {
+    // Use SCS as the last resort. SCS uses ADMM method, which converges fast to
+    // modest accuracy quite fast, but then slows down significantly if the user
+    // wants high accuracy.
+    return scs_solver_->Solve(*this);
   } else {
     throw runtime_error(
         "MathematicalProgram::Solve: "

diff --git a/drake/solvers/mathematical_program.h b/drake/solvers/mathematical_program.h
@@ -90,6 +90,14 @@ namespace solvers {
  *    <td align="center">&diams;</td>
  *    <td align="center">&diams;</td>
  * </tr>
+ * <tr><td>&dagger; <a href="https://github.com/cvxgrp/scs">
+ *    SCS</a></td>
+ *    <td align="center">&diams;</td>
+ *    <td></td>
+ *    <td></td>
+ *    <td></td>
+ *    <td></td>
+ * </tr>
  * </table>
  *
  * <b>Mixed-Integer Convex Optimization</b>
@@ -2283,6 +2291,7 @@ class MathematicalProgram {
       equality_constrained_qp_solver_;
   std::unique_ptr<MathematicalProgramSolverInterface> gurobi_solver_;
   std::unique_ptr<MathematicalProgramSolverInterface> mosek_solver_;
+  std::unique_ptr<MathematicalProgramSolverInterface> scs_solver_;
 
   template <typename T>
   void NewVariables_impl(