Logarithmic coding of special ordered set 2 constraint (#6337)

* start to add the logarithmic sos2 constraints

* WIP: add logarithmic sos2 constraint

* Fix the bug, now sos2 works

* Add more tests on sos2

* cpplint

* more documentation

* buildifier

* Address Greg's comments

* create a separate gray code file

* Have a separate gray_code library

* cpplint

* minor changes

* remove the gray code from mixed_integer_optimization_util

* add documentation

* Add a separate interface to AddLogarithmicSOS2Constraint, add the binary variables as an input instead of a return argument

drake/math/BUILD

@@ -125,6 +125,16 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "gray_code",
+    srcs = ["gray_code.cc"],
+    hdrs = ["gray_code.h"],
+    deps = [
+        "@eigen",
+        "//drake/common",
+    ],
+)
+
 drake_cc_library(
     name = "jacobian",
     srcs = ["jacobian.cc"],
@@ -214,6 +224,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "gray_code_test",
+    deps = [
+        ":gray_code",
+        "//drake/common:eigen_matrix_compare",
+    ],
+)
+
 drake_cc_googletest(
     name = "matrix_util_test",
     deps = [

drake/math/CMakeLists.txt

@@ -10,6 +10,7 @@ set(sources
   evenly_distributed_pts_on_sphere.cc
   expmap.cc
   gradient.cc
+  gray_code.cc
   jacobian.cc
   matrix_util.cc
   normalize_vector.cc
@@ -32,6 +33,7 @@ set(installed_headers
   expmap.h
   gradient.h
   gradient_util.h
+  gray_code.h
   jacobian.h
   matrix_util.h
   normalize_vector.h

drake/math/gray_code.cc

@@ -0,0 +1,17 @@
+#include "drake/math/gray_code.h"
+
+namespace drake {
+namespace math {
+int GrayCodeToInteger(const Eigen::Ref<const Eigen::VectorXi>& gray_code) {
+  // This implementation is based on
+  // https://testbook.com/blog/conversion-from-gray-code-to-binary-code-and-vice-versa/
+  int digit = gray_code(0);
+  int ret = digit ? 1 << (gray_code.size() - 1) : 0;
+  for (int i = 0; i < gray_code.size() - 1; ++i) {
+    digit ^= gray_code(i + 1);
+    ret |= digit ? 1 << (gray_code.size() - i - 2) : 0;
+  }
+  return ret;
+}
+}  // namespace math
+}  // namespace drake

drake/math/gray_code.h

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <Eigen/Core>
+#include "drake/common/drake_assert.h"
+
+namespace drake {
+namespace math {
+/**
+ * GrayCodesMatrix::type returns an Eigen matrix of integers. The size of this
+ * matrix is determined by the number of digits in the Gray code.
+ */
+template<int NumDigits>
+struct GrayCodesMatrix {
+  static_assert(NumDigits >= 0 && NumDigits <= 30, "NumDigits out of range.");
+  typedef typename Eigen::Matrix<int, NumDigits == 0 ? 0 : 1 << NumDigits,
+                                 NumDigits>
+      type;
+};
+
+template<>
+struct GrayCodesMatrix<Eigen::Dynamic> {
+  typedef typename Eigen::MatrixXi type;
+};
+
+/**
+ * Returns a matrix whose i'th row is the Gray code for integer i.
+ * @tparam NumDigits The number of digits in the Gray code.
+ * @param num_digits The number of digits in the Gray code.
+ * @return M. M is a matrix of size 2ᵏ x k, where `k` is `num_digits`.
+ * M.row(i) is the Gray code for integer i.
+ */
+template<int NumDigits = Eigen::Dynamic>
+typename GrayCodesMatrix<NumDigits>::type
+CalculateReflectedGrayCodes(int num_digits = NumDigits) {
+  DRAKE_DEMAND(num_digits >= 0);
+  int num_codes = num_digits <= 0 ? 0 : 1 << num_digits;
+  typename GrayCodesMatrix<NumDigits>::type gray_codes(num_codes, num_digits);
+  // TODO(hongkai.dai): implement this part more efficiently.
+  for (int i = 0; i < num_codes; ++i) {
+    int gray_code = i ^ (i >> 1);
+    for (int j = 0; j < num_digits; ++j) {
+      gray_codes(i, j) =
+          (gray_code & (1 << (num_digits - j - 1))) >> (num_digits - j - 1);
+    }
+  }
+  return gray_codes;
+}
+
+/**
+ * Converts the Gray code to an integer. For example
+ * (0, 0) -> 0
+ * (0, 1) -> 1
+ * (1, 1) -> 2
+ * (1, 0) -> 3
+ * @param gray_code The N-digit Gray code, where N is gray_code.rows()
+ * @return The integer represented by the Gray code `gray_code`.
+ */
+int GrayCodeToInteger(const Eigen::Ref<const Eigen::VectorXi>& gray_code);
+}  // namespace math
+}  // namespace drake

drake/math/test/CMakeLists.txt

@@ -32,4 +32,7 @@ drake_add_cc_test(jacobian_test)
 target_link_libraries(jacobian_test drakeCommon)
 
 drake_add_cc_test(matrix_util_test)
-target_linK_libraries(matrix_util_test drakeCommon)
+target_link_libraries(matrix_util_test drakeCommon)
+
+drake_add_cc_test(gray_code_test)
+target_link_libraries(gray_code_test drakeCommon drakeMath)

drake/math/test/gray_code_test.cc

@@ -0,0 +1,92 @@
+#include "drake/math/gray_code.h"
+
+#include <gtest/gtest.h>
+
+#include "drake/common/eigen_matrix_compare.h"
+
+namespace drake {
+namespace math {
+namespace {
+GTEST_TEST(TestGrayCode, TestCalculateGrayCodes) {
+  for (int i = 0; i < 4; i++) {
+    auto test_code = CalculateReflectedGrayCodes(i);
+    // Asking for codes for 0 bits should generate 0 for 0 bits.
+    // Asking for codes for i bits should generate 2^(i) codes for i bits.
+    EXPECT_EQ(test_code.cols(), i);
+    EXPECT_EQ(test_code.rows(), i == 0 ? 0 : 1 << i);
+    // Each code should differ by only one bit from the previous code.
+    for (int j = 1; j < test_code.rows(); j ++) {
+      EXPECT_EQ((test_code.row(j) - test_code.row(j - 1)).cwiseAbs().sum(), 1);
+    }
+  }
+}
+
+template <int NumDigits, int NumCodes>
+void TestGrayCode(const Eigen::Ref<const Eigen::MatrixXi>& gray_codes) {
+  auto gray_codes_dynamic = CalculateReflectedGrayCodes(NumDigits);
+  auto gray_codes_static = CalculateReflectedGrayCodes<NumDigits>();
+  static_assert(
+      std::is_same<decltype(gray_codes_dynamic), Eigen::MatrixXi>::value,
+      "Should be a dynamic sized matrix");
+  static_assert(std::is_same<decltype(gray_codes_static),
+                             Eigen::Matrix<int, NumCodes, NumDigits>>::value,
+                "Should be a static sized matrix");
+  EXPECT_TRUE(CompareMatrices(gray_codes, gray_codes_dynamic, 1E-5,
+                              MatrixCompareType::absolute));
+  EXPECT_TRUE(CompareMatrices(gray_codes, gray_codes_static, 1E-5,
+                              MatrixCompareType::absolute));
+}
+
+GTEST_TEST(TestGrayCode, TestCalculateGrayCodes0) {
+  Eigen::Matrix<int, 0, 0> gray_codes;
+  TestGrayCode<0, 0>(gray_codes);
+}
+
+GTEST_TEST(TestGrayCode, TestCalculateGrayCodes1) {
+  Eigen::Matrix<int, 2, 1> gray_codes(0, 1);
+  TestGrayCode<1, 2>(gray_codes);
+}
+
+GTEST_TEST(TestGrayCode, TestCalculateGrayCodes2) {
+  Eigen::Matrix<int, 4, 2> gray_codes;
+  // clang-format off
+  gray_codes << 0, 0,
+                0, 1,
+                1, 1,
+                1, 0;
+  // clang-format on
+  TestGrayCode<2, 4>(gray_codes);
+}
+
+GTEST_TEST(TestGrayCode, TestCalculateGrayCodes3) {
+  Eigen::Matrix<int, 8, 3> gray_codes;
+  // clang-format off
+  gray_codes << 0, 0, 0,
+                0, 0, 1,
+                0, 1, 1,
+                0, 1, 0,
+                1, 1, 0,
+                1, 1, 1,
+                1, 0, 1,
+                1, 0, 0;
+  // clang-format on
+  TestGrayCode<3, 8>(gray_codes);
+}
+
+GTEST_TEST(TestGrayCode, TestGrayCodeToInteger) {
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector2i(0, 0)), 0);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector2i(0, 1)), 1);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector2i(1, 1)), 2);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector2i(1, 0)), 3);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector3i(0, 0, 0)), 0);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector3i(0, 0, 1)), 1);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector3i(0, 1, 1)), 2);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector3i(0, 1, 0)), 3);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector3i(1, 1, 0)), 4);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector3i(1, 1, 1)), 5);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector3i(1, 0, 1)), 6);
+  EXPECT_EQ(GrayCodeToInteger(Eigen::Vector3i(1, 0, 0)), 7);
+}
+}  // namespace
+}  // namespace math
+}  // namespace drake

drake/solvers/BUILD

@@ -513,6 +513,16 @@ drake_cc_library(
     }),
 )
 
+drake_cc_library(
+    name = "mixed_integer_optimization_util",
+    srcs = ["mixed_integer_optimization_util.cc"],
+    hdrs = ["mixed_integer_optimization_util.h"],
+    deps = [
+        ":mathematical_program",
+        "//drake/math:gray_code",
+    ],
+)
+
 # === test/ ===
 drake_cc_googletest(
     name = "binding_test",
@@ -798,6 +808,16 @@ drake_cc_binary(
     ],
 )
 
+drake_cc_googletest(
+    name = "mixed_integer_optimization_util_test",
+    tags = gurobi_test_tags(gurobi_required = False),
+    deps = [
+        ":gurobi_solver",
+        ":mixed_integer_optimization_util",
+        "//drake/common:eigen_matrix_compare",
+    ],
+)
+
 # The extra_srcs are required here because cpplint() doesn't understand how to
 # extract labels from select() functions yet.
 cpplint(extra_srcs = [

drake/solvers/CMakeLists.txt

@@ -139,6 +139,14 @@ if(mosek_FOUND)
   target_link_libraries(drakeOptimization mosek)
 endif()
 
+add_library_with_exports(LIB_NAME drakeMixedIntegerOptimizationUtil SOURCE_FILES mixed_integer_optimization_util.cc)
+target_link_libraries(drakeMixedIntegerOptimizationUtil
+        drakeMath
+        drakeOptimization)
+drake_install_headers(
+        mixed_integer_optimization_util.h
+)
+
 add_library_with_exports(LIB_NAME drakeRotationConstraint SOURCE_FILES rotation_constraint.cc)
 target_link_libraries(drakeRotationConstraint
         drakeOptimization)

drake/solvers/mixed_integer_optimization_util.cc

@@ -0,0 +1,52 @@
+#include "drake/solvers/mixed_integer_optimization_util.h"
+
+#include "drake/math/gray_code.h"
+
+namespace drake {
+namespace solvers {
+VectorXDecisionVariable AddLogarithmicSOS2Constraint(
+    MathematicalProgram* prog,
+    const Eigen::Ref<const VectorX<symbolic::Expression>>& lambda,
+    const std::string& binary_variable_name) {
+  const int num_lambda = lambda.rows();
+  const int num_interval = num_lambda - 1;
+  const int num_binary_vars = CeilLog2(num_interval);
+  auto y = prog->NewBinaryVariables(num_binary_vars, binary_variable_name);
+  AddLogarithmicSOS2Constraint(prog, lambda, y);
+  return y;
+}
+
+void AddLogarithmicSOS2Constraint(
+    MathematicalProgram* prog,
+    const Eigen::Ref<const VectorX<symbolic::Expression>>& lambda,
+    const Eigen::Ref<const VectorXDecisionVariable>& y) {
+  const int num_lambda = lambda.rows();
+  for (int i = 0; i < num_lambda; ++i) {
+    prog->AddLinearConstraint(lambda(i) >= 0);
+    prog->AddLinearConstraint(lambda(i) <= 1);
+  }
+  prog->AddLinearConstraint(lambda.sum() == 1);
+  const int num_interval = num_lambda - 1;
+  const int num_binary_vars = CeilLog2(num_interval);
+  const auto gray_codes =
+      math::CalculateReflectedGrayCodes(num_binary_vars);
+  DRAKE_ASSERT(y.rows() == num_binary_vars);
+  for (int j = 0; j < num_binary_vars; ++j) {
+    symbolic::Expression lambda_sum1 = gray_codes(0, j) == 1 ? lambda(0) : 0;
+    symbolic::Expression lambda_sum2 = gray_codes(0, j) == 0 ? lambda(0) : 0;
+    for (int i = 1; i < num_lambda - 1; ++i) {
+      lambda_sum1 +=
+          (gray_codes(i - 1, j) == 1 && gray_codes(i, j) == 1) ? lambda(i) : 0;
+      lambda_sum2 +=
+          (gray_codes(i - 1, j) == 0 && gray_codes(i, j) == 0) ? lambda(i) : 0;
+    }
+    lambda_sum1 +=
+        gray_codes(num_lambda - 2, j) == 1 ? lambda(num_lambda - 1) : 0;
+    lambda_sum2 +=
+        gray_codes(num_lambda - 2, j) == 0 ? lambda(num_lambda - 1) : 0;
+    prog->AddLinearConstraint(lambda_sum1 <= y(j));
+    prog->AddLinearConstraint(lambda_sum2 <= 1 - y(j));
+  }
+}
+}  // namespace solvers
+}  // namespace drake

drake/solvers/mixed_integer_optimization_util.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <string>
+
+#include "drake/solvers/mathematical_program.h"
+
+namespace drake {
+namespace solvers {
+/**
+ * Return ⌈log₂(n)⌉, namely the minimal integer no smaller than log₂(n), with
+ * base 2.
+ * @param n A positive integer.
+ * @return The minimal integer no smaller than log₂(n).
+ */
+constexpr int CeilLog2(int n) {
+  return n == 1 ? 0 : 1 + CeilLog2((n + 1) / 2);
+}
+
+/**
+ * Adds the special ordered set 2 (sos2) constraint, that at most two
+ * entries in λ can be strictly positive, and these two entries have to be
+ * adjacent. All other λ should be zero. Moreover, the non-zero λ satisfies
+ * λ(i) + λ(i + 1) = 1.
+ * We will need to add ⌈log₂(n - 1)⌉ binary variables, where n is the number of
+ * rows in λ. For more information, please refer to
+ *   Modeling Disjunctive Constraints with a Logarithmic Number of Binary
+ *   Variables and Constraints
+ *   by J. Vielma and G. Nemhauser, 2011.
+ * @param prog Add the sos2 constraint to this mathematical program.
+ * @param lambda At most two entries in λ can be strictly positive, and these
+ * two entries have to be adjacent. All other entries are zero.
+ * @return y The newly added binary variables. The assignment of the binary
+ * variable y implies which two λ can be strictly positive.
+ * With a binary assignment on y, and suppose the integer M corresponds to
+ * (y(0), y(1), ..., y(⌈log₂(n - 1)⌉)) in Gray code, then only λ(M) and λ(M + 1)
+ * can be non-zero. For example, if the assignment of y = (1, 1), in Gray code,
+ * (1, 1) represents integer 2, so only λ(2) and λ(3) can be strictly positive.
+ */
+VectorXDecisionVariable AddLogarithmicSOS2Constraint(
+    MathematicalProgram* prog,
+    const Eigen::Ref<const VectorX<symbolic::Expression>>& lambda,
+    const std::string& binary_variable_name = "y");
+
+/** Adds the special ordered set 2 (sos2) constraint, @see AddLogarithmicSOS2Constraint.
+ */
+void AddLogarithmicSOS2Constraint(
+    MathematicalProgram* prog,
+    const Eigen::Ref<const VectorX<symbolic::Expression>>& lambda,
+    const Eigen::Ref<const VectorXDecisionVariable>& y);
+}  // namespace solvers
+}  // namespace drake

drake/solvers/test/CMakeLists.txt

@@ -72,6 +72,9 @@ target_link_libraries(gurobi_solver_test drakeCommon drakeOptimizationTest)
 drake_add_cc_test(mosek_solver_test)
 target_link_libraries(mosek_solver_test drakeCommon drakeOptimizationTest)
 
+drake_add_cc_test(mixed_integer_optimization_util_test)
+target_link_libraries(mixed_integer_optimization_util_test drakeCommon drakeMixedIntegerOptimizationUtil)
+
 drake_add_cc_test(linear_complementary_problem_test)
 target_link_libraries(linear_complementary_problem_test drakeOptimization)