Add python binding for ReplaceBilinearTerms (RobotLocomotion#17707)

* Add python binding for ReplaceBilinearTerms
Move ReplaceBilinearTerms from solvers to symbolic namespace.
Deprecate solvers::ReplaceBilinearTerms

bindings/pydrake/symbolic_py.cc

@@ -17,6 +17,7 @@
 #include "drake/common/symbolic/decompose.h"
 #include "drake/common/symbolic/latex.h"
 #include "drake/common/symbolic/monomial_util.h"
+#include "drake/common/symbolic/replace_bilinear_terms.h"
 #include "drake/common/symbolic/trigonometric_polynomial.h"
 
 namespace drake {
@@ -1129,6 +1130,10 @@ PYBIND11_MODULE(symbolic, m) {
           py::arg("f"), py::arg("parameters"),
           doc.DecomposeLumpedParameters.doc);
 
+  // Bind free function in replace_bilinear_terms.
+  m.def("ReplaceBilinearTerms", &ReplaceBilinearTerms, py::arg("e"),
+      py::arg("x"), py::arg("y"), py::arg("W"), doc.ReplaceBilinearTerms.doc);
+
   // NOLINTNEXTLINE(readability/fn_size)
 }
 }  // namespace pydrake

bindings/pydrake/test/symbolic_test.py

@@ -2288,3 +2288,17 @@ def test(self):
             sym.Polynomial(4*tx*(1+ty*ty) + 2*ty * (1-tx*tx)).Expand()))
         self.assertTrue(r.denominator().Expand().EqualTo(
             sym.Polynomial((1+ty*ty)*(1+tx*tx)).Expand()))
+
+
+class TestReplaceBilinearTerms(unittest.TestCase):
+    def test(self):
+        x = np.array([sym.Variable("x0"), sym.Variable("x1")])
+        y = np.array([
+            sym.Variable("y0"), sym.Variable("y1"), sym.Variable("y2")])
+        W = np.empty((2, 3), dtype=object)
+        for i in range(2):
+            for j in range(3):
+                W[i, j] = sym.Variable(f"W({i}, {j})")
+        e = x[0]*y[1] * 3 + x[1]*y[2] * 4
+        e_replace = sym.ReplaceBilinearTerms(e=e, x=x, y=y, W=W)
+        self.assertTrue(e_replace.EqualTo(W[0, 1]*3 + W[1, 2]*4))

common/symbolic/BUILD.bazel

@@ -23,6 +23,7 @@ drake_cc_package_library(
         ":polynomial",
         ":polynomial_basis",
         ":rational_function",
+        ":replace_bilinear_terms",
         ":simplification",
         ":trigonometric_polynomial",
     ],
@@ -253,6 +254,27 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_library(
+    name = "replace_bilinear_terms",
+    srcs = ["replace_bilinear_terms.cc"],
+    hdrs = ["replace_bilinear_terms.h"],
+    interface_deps = [
+        "//common:essential",
+        "//common/symbolic:expression",
+    ],
+    deps = [
+        "//common/symbolic:polynomial",
+    ],
+)
+
+drake_cc_googletest(
+    name = "replace_bilinear_terms_test",
+    deps = [
+        ":replace_bilinear_terms",
+        "//common/test_utilities:symbolic_test_util",
+    ],
+)
+
 drake_cc_library(
     name = "simplification",
     srcs = ["simplification.cc"],

common/symbolic/replace_bilinear_terms.cc

@@ -0,0 +1,128 @@
+#include "drake/common/symbolic/replace_bilinear_terms.h"
+
+#include <ostream>
+#include <sstream>
+#include <stdexcept>
+#include <unordered_map>
+
+#include "drake/common/symbolic/polynomial.h"
+
+namespace drake {
+namespace symbolic {
+using std::ostringstream;
+using std::runtime_error;
+using std::unordered_map;
+
+using MapVarToIndex = unordered_map<Variable::Id, int>;
+
+/*
+ * Returns the map that maps x(i).get_id() to i.
+ */
+MapVarToIndex ConstructVarToIndexMap(
+    const Eigen::Ref<const VectorX<symbolic::Variable>>& x) {
+  MapVarToIndex map;
+  map.reserve(x.rows());
+  for (int i = 0; i < x.rows(); ++i) {
+    const auto it = map.find(x(i).get_id());
+    if (it != map.end()) {
+      throw runtime_error("Input vector contains duplicate variable " +
+                          x(i).get_name());
+    }
+    map.emplace_hint(it, x(i).get_id(), i);
+  }
+  return map;
+}
+
+Expression ReplaceBilinearTerms(
+    const Expression& e, const Eigen::Ref<const VectorX<symbolic::Variable>>& x,
+    const Eigen::Ref<const VectorX<symbolic::Variable>>& y,
+    const Eigen::Ref<const MatrixX<symbolic::Expression>>& W) {
+  DRAKE_ASSERT(W.rows() == x.rows() && W.cols() == y.rows());
+  const MapVarToIndex x_to_index_map = ConstructVarToIndexMap(x);
+  const MapVarToIndex y_to_index_map = ConstructVarToIndexMap(y);
+
+  // p_bilinear is a polynomial with x and y as indeterminates.
+  const Variables variables_in_x_y{Variables{x} + Variables{y}};
+  const symbolic::Polynomial p_bilinear{e, variables_in_x_y};
+  const auto& map_bilinear = p_bilinear.monomial_to_coefficient_map();
+  symbolic::Polynomial poly;
+  for (const auto& p : map_bilinear) {
+    MapVarToIndex monomial_map;
+    const int monomial_degree = p.first.total_degree();
+    for (const auto& var_power : p.first.get_powers()) {
+      monomial_map.emplace(var_power.first.get_id(), var_power.second);
+    }
+    if (monomial_degree > 2) {
+      ostringstream oss;
+      oss << "The term " << p.first
+          << " has degree larger than 2 on the variables";
+      throw runtime_error(oss.str());
+    } else if (monomial_degree < 2) {
+      // Only linear or constant terms, do not need to replace the variables.
+      poly.AddProduct(p.second, p.first);
+    } else {
+      // This monomial contains bilinear term in x and y.
+      MapVarToIndex::const_iterator it_x_idx;
+      MapVarToIndex::const_iterator it_y_idx;
+      if (monomial_map.size() == 2) {
+        // The monomial is in the form of x * y, namely two different
+        // variables multiplying together.
+        auto monomial_map_it = monomial_map.begin();
+        const Variable::Id var1_id{monomial_map_it->first};
+        ++monomial_map_it;
+        const Variable::Id var2_id{monomial_map_it->first};
+        it_x_idx = x_to_index_map.find(var1_id);
+        if (it_x_idx != x_to_index_map.end()) {
+          // var1 is in x.
+          it_y_idx = y_to_index_map.find(var2_id);
+        } else {
+          // var1 is in y.
+          it_x_idx = x_to_index_map.find(var2_id);
+          it_y_idx = y_to_index_map.find(var1_id);
+        }
+      } else {
+        // The monomial is in the form of x * x, the square of a variable.
+        const Variable::Id squared_var_id{monomial_map.begin()->first};
+        it_x_idx = x_to_index_map.find(squared_var_id);
+        it_y_idx = y_to_index_map.find(squared_var_id);
+      }
+      if (it_x_idx == x_to_index_map.end() ||
+          it_y_idx == y_to_index_map.end()) {
+        // This error would happen, if we ask
+        // ReplaceBilinearTerms(x(i) * x(j), x, y, W).
+        ostringstream oss;
+        oss << "Term " << p.first
+            << " is bilinear, but x and y does not have "
+               "the corresponding variables.";
+        throw runtime_error(oss.str());
+      }
+      // w_xy_expr is the symbolic expression representing the bilinear term x *
+      // y.
+      const symbolic::Expression& w_xy_expr{
+          W(it_x_idx->second, it_y_idx->second)};
+      if (is_variable(w_xy_expr)) {
+        const symbolic::Variable w_xy = symbolic::get_variable(w_xy_expr);
+        if (variables_in_x_y.include(w_xy)) {
+          // Case: w_xy is an indeterminate.
+          poly.AddProduct(p.second, symbolic::Monomial{w_xy});
+        } else {
+          // Case: w_xy is a decision variable.
+          poly.AddProduct(w_xy * p.second, symbolic::Monomial{});
+        }
+      } else {
+        if (intersect(w_xy_expr.GetVariables(), variables_in_x_y).size() != 0) {
+          // w_xy_expr contains a variable in x or y.
+          ostringstream oss;
+          oss << "W(" + std::to_string(it_x_idx->second) + "," +
+                     std::to_string(it_y_idx->second) + ")="
+              << w_xy_expr << "contains variables in x or y.";
+          throw std::runtime_error(oss.str());
+        }
+        poly.AddProduct(w_xy_expr * p.second, symbolic::Monomial{});
+      }
+    }
+  }
+  return poly.ToExpression();
+}
+}  // namespace symbolic
+}  // namespace drake

common/symbolic/replace_bilinear_terms.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include "drake/common/eigen_types.h"
+#include "drake/common/symbolic/expression.h"
+
+namespace drake {
+namespace symbolic {
+/**
+ * Replaces all the bilinear product terms in the expression `e`, with the
+ * corresponding terms in `W`, where `W` represents the matrix x * yᵀ, such that
+ * after replacement, `e` does not have bilinear terms involving `x` and `y`.
+ * For example, if e = x(0)*y(0) + 2 * x(0)*y(1) + x(1) * y(1) + 3 * x(1), `e`
+ * has bilinear terms x(0)*y(0), x(0) * y(1) and x(2) * y(1), if we call
+ * ReplaceBilinearTerms(e, x, y, W) where W(i, j) represent the term x(i) *
+ * y(j), then this function returns W(0, 0) + 2 * W(0, 1) + W(1, 1) + 3 * x(1).
+ * @param e An expression potentially contains bilinear products between x and
+ * y.
+ * @param x The bilinear product between `x` and `y` will be replaced by the
+ * corresponding term in `W`.
+ * @throws std::exception if `x` contains duplicate entries.
+ * @param y The bilinear product between `x` and `y` will be replaced by the
+ * corresponding term in `W.
+ * @throws std::exception if `y` contains duplicate entries.
+ * @param W Bilinear product term x(i) * y(j) will be replaced by W(i, j). If
+ * W(i,j) is not a single variable, but an expression, then this expression
+ * cannot contain a variable in either x or y.
+ * @throws std::exception, if W(i, j) is not a single variable, and also
+ * contains a variable in x or y.
+ * @pre W.rows() == x.rows() and W.cols() == y.rows().
+ * @return The symbolic expression after replacing x(i) * y(j) with W(i, j).
+ */
+symbolic::Expression ReplaceBilinearTerms(
+    const symbolic::Expression& e,
+    const Eigen::Ref<const VectorX<symbolic::Variable>>& x,
+    const Eigen::Ref<const VectorX<symbolic::Variable>>& y,
+    const Eigen::Ref<const MatrixX<symbolic::Expression>>& W);
+}  // namespace symbolic
+}  // namespace drake