Compute monomial basis with the degree for each variable <= 1. (Robot…

…Locomotion#18222)

bindings/pydrake/symbolic_py.cc

@@ -770,7 +770,11 @@ PYBIND11_MODULE(symbolic, m) {
       .def("EvenDegreeMonomialBasis", &symbolic::EvenDegreeMonomialBasis,
           py::arg("vars"), py::arg("degree"), doc.EvenDegreeMonomialBasis.doc)
       .def("OddDegreeMonomialBasis", &symbolic::OddDegreeMonomialBasis,
-          py::arg("vars"), py::arg("degree"), doc.OddDegreeMonomialBasis.doc);
+          py::arg("vars"), py::arg("degree"), doc.OddDegreeMonomialBasis.doc)
+      .def("CalcMonomialBasisOrderUpToOne",
+          &symbolic::CalcMonomialBasisOrderUpToOne, py::arg("x"),
+          py::arg("sort_monomial") = false,
+          doc.CalcMonomialBasisOrderUpToOne.doc);
 
   using symbolic::Polynomial;
 

bindings/pydrake/test/symbolic_test.py

@@ -1152,6 +1152,11 @@ def test_off_degree_monomial_basis(self):
         basis = sym.OddDegreeMonomialBasis(vars, 3)
         self.assertEqual(basis.size, 6)
 
+    def test_calc_monomial_basis_order_up_to_one(self):
+        basis = sym.CalcMonomialBasisOrderUpToOne(
+            x=sym.Variables([x, y, z]), sort_monomial=False)
+        self.assertEqual(basis.size, 8)
+
     def test_evaluate(self):
         m = sym.Monomial(x, 3) * sym.Monomial(y)  # m = x³y
         env = {x: 2.0,

common/symbolic/BUILD.bazel

@@ -156,6 +156,13 @@ drake_cc_library(
     ],
 )
 
+drake_cc_googletest(
+    name = "monomial_util_test",
+    deps = [
+        ":monomial_util",
+    ],
+)
+
 drake_cc_library(
     name = "polynomial",
     srcs = [

common/symbolic/monomial_util.cc

@@ -1,5 +1,8 @@
 #include "drake/common/symbolic/monomial_util.h"
 
+#include <algorithm>
+#include <vector>
+
 #include "drake/common/drake_assert.h"
 
 namespace drake {
@@ -21,5 +24,61 @@ Eigen::Matrix<Monomial, Eigen::Dynamic, 1> OddDegreeMonomialBasis(
   return internal::ComputeMonomialBasis<Eigen::Dynamic>(
       vars, degree, internal::DegreeType::kOdd);
 }
+
+namespace {
+// Generates all the monomials with degree up to 1 for each variable, for all
+// the variables in x[start], x[start+1], ..., x.back(). Append these monomials
+// to `monomial_basis`.
+// @param[in/out] monomial_basis We assume that as the input, monomial_basis
+// doesn't contain x[start] in its variables. As an output, the new monomials
+// that contain x[start] will be appended to monomial_basis.
+void CalcMonomialBasisOrderUpToOneHelper(
+    const std::vector<Variable>& x, int start,
+    std::vector<Monomial>* monomial_basis) {
+  DRAKE_DEMAND(start >= 0 && start < static_cast<int>(x.size()));
+  if (start == static_cast<int>(x.size() - 1)) {
+    // Generate the monomial for a single variable, which is 1 and the variable
+    // itself.
+    monomial_basis->emplace_back();
+    monomial_basis->emplace_back(x[start], 1);
+    return;
+  } else {
+    // First generate the monomials using all the variables x[start + 1] ...
+    // x[x.size() - 1]
+    CalcMonomialBasisOrderUpToOneHelper(x, start + 1, monomial_basis);
+    const int monomial_count = monomial_basis->size();
+    // Construct the monomial x[start].
+    const Monomial x_start(x[start], 1);
+    // Multiply x_start to each monomial already in monomial_set, and append the
+    // multiplication result to monomial_set.
+    for (int i = 0; i < monomial_count; ++i) {
+      monomial_basis->push_back(x_start * (*monomial_basis)[i]);
+    }
+    return;
+  }
+}
+}  // namespace
+
+VectorX<Monomial> CalcMonomialBasisOrderUpToOne(const Variables& x,
+                                                bool sort_monomial) {
+  std::vector<Variable> x_vec;
+  x_vec.reserve(x.size());
+  for (const auto& var : x) {
+    x_vec.push_back(var);
+  }
+  std::vector<Monomial> monomial_basis;
+  // There will be pow(2, x.size()) monomials in the basis.
+  monomial_basis.reserve(1 << static_cast<int>(x.size()));
+  CalcMonomialBasisOrderUpToOneHelper(x_vec, 0, &monomial_basis);
+  VectorX<Monomial> ret(monomial_basis.size());
+  if (sort_monomial) {
+    std::sort(monomial_basis.begin(), monomial_basis.end(),
+              GradedReverseLexOrder<std::less<Variable>>());
+  }
+  for (int i = 0; i < ret.rows(); ++i) {
+    ret(i) = monomial_basis[i];
+  }
+  return ret;
+}
 }  // namespace symbolic
 }  // namespace drake

common/symbolic/monomial_util.h

@@ -241,5 +241,16 @@ Eigen::Matrix<Monomial, Eigen::Dynamic, 1> EvenDegreeMonomialBasis(
 Eigen::Matrix<Monomial, Eigen::Dynamic, 1> OddDegreeMonomialBasis(
     const Variables& vars, int degree);
 
+/**
+ Generates all the monomials of `x`, such that the degree for x(i) is no larger
+ than 1 for every x(i) in `x`.
+ @param t The variables whose monomials are generated.
+ @param sort_monomial If true, the returned monomials are sorted in the graded
+ reverse lexicographic order. For example if x = (x₀, x₁) with x₀< x₁, then this
+ function returns [x₀x₁, x₁, x₀, 1]. If sort_monomial=false, then we return the
+ monomials in an arbitrary order.
+ */
+[[nodiscard]] VectorX<Monomial> CalcMonomialBasisOrderUpToOne(
+    const Variables& x, bool sort_monomial = false);
 }  // namespace symbolic
 }  // namespace drake

common/symbolic/test/monomial_util_test.cc

@@ -0,0 +1,63 @@
+#include "drake/common/symbolic/monomial_util.h"
+
+#include <unordered_set>
+
+#include <gtest/gtest.h>
+
+namespace drake {
+namespace symbolic {
+void CheckCalcMonomialBasisOrderUpToOne(const drake::symbolic::Variables& t) {
+  // First compute the unsorted vector of monomial basis.
+  const auto basis_unsorted = CalcMonomialBasisOrderUpToOne(t, false);
+  const int basis_size_expected =
+      static_cast<int>(std::pow(2, static_cast<int>(t.size())));
+  EXPECT_EQ(basis_unsorted.rows(), basis_size_expected);
+  std::unordered_set<Monomial> basis_set;
+  basis_set.reserve(basis_size_expected);
+  for (int i = 0; i < basis_unsorted.rows(); ++i) {
+    for (const Variable& ti : t) {
+      EXPECT_LE(basis_unsorted(i).degree(ti), 1);
+    }
+    basis_set.insert(basis_unsorted(i));
+  }
+  // Make sure basis_set has the same size as basis_unsorted. Hence each element
+  // inside basis_unsorted is unique.
+  EXPECT_EQ(basis_set.size(), basis_size_expected);
+
+  // Now compute the sorted vector of monomial basis.
+  const auto basis_sorted = CalcMonomialBasisOrderUpToOne(t, true);
+  EXPECT_EQ(basis_sorted.rows(), basis_size_expected);
+  for (int i = 0; i < basis_sorted.rows(); ++i) {
+    EXPECT_EQ(basis_set.count(basis_sorted(i)), 1);
+  }
+  // Make sure that basis_sorted is actually in the graded lexicographic order.
+  for (int i = 0; i < basis_sorted.rows() - 1; ++i) {
+    EXPECT_TRUE(GradedReverseLexOrder<std::less<Variable>>()(
+        basis_sorted(i), basis_sorted(i + 1)));
+  }
+}
+
+class CalcMonomialBasisTest : public ::testing::Test {
+ protected:
+  Variable t1_{"t1"};
+  Variable t2_{"t2"};
+  Variable t3_{"t3"};
+  Variable t4_{"t4"};
+};
+
+TEST_F(CalcMonomialBasisTest, CalcMonomialBasisOrderUpToOne) {
+  CheckCalcMonomialBasisOrderUpToOne(Variables({t1_}));
+  CheckCalcMonomialBasisOrderUpToOne(Variables({t1_, t2_}));
+  CheckCalcMonomialBasisOrderUpToOne(Variables({t1_, t2_, t3_}));
+  CheckCalcMonomialBasisOrderUpToOne(Variables({t1_, t2_, t3_, t4_}));
+
+  const Vector4<symbolic::Monomial> monomial12 =
+      CalcMonomialBasisOrderUpToOne(Variables({t1_, t2_}), true);
+  ASSERT_TRUE(std::less<Variable>()(t1_, t2_));
+  EXPECT_EQ(monomial12[0], Monomial(t1_ * t2_));
+  EXPECT_EQ(monomial12[1], Monomial(t2_));
+  EXPECT_EQ(monomial12[2], Monomial(t1_));
+  EXPECT_EQ(monomial12[3], Monomial(1));
+}
+}  // namespace symbolic
+}  // namespace drake