Generic polynomial (RobotLocomotion#13940)

Add simple constructors for GenericPolynomial, and Differentiate function.
The follow-on PRs will add more functionality to GenericPolynomial class. Eventually we will replace Polynomial class with GenericPolynomial<MonomialBasisElement> class.

common/BUILD.bazel

@@ -202,6 +202,8 @@ drake_cc_library(
         "symbolic_formula_cell.cc",
         "symbolic_formula_cell.h",
         "symbolic_formula_visitor.h",
+        "symbolic_generic_polynomial.cc",
+        "symbolic_generic_polynomial.h",
         "symbolic_ldlt.cc",
         "symbolic_ldlt.h",
         "symbolic_monomial.cc",
@@ -1179,6 +1181,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "symbolic_generic_polynomial_test",
+    deps = [
+        ":symbolic",
+        "//common/test_utilities:expect_no_throw",
+        "//common/test_utilities:expect_throws_message",
+        "//common/test_utilities:symbolic_test_util",
+    ],
+)
+
 drake_cc_googletest(
     name = "symbolic_polynomial_test",
     deps = [

common/symbolic.h

@@ -43,6 +43,7 @@
 #include "drake/common/symbolic_monomial_basis_element.h"
 #include "drake/common/symbolic_chebyshev_polynomial.h"
 #include "drake/common/symbolic_polynomial_basis.h"
+#include "drake/common/symbolic_generic_polynomial.h"
 #include "drake/common/symbolic_rational_function.h"
 #include "drake/common/symbolic_formula.h"
 #include "drake/common/symbolic_formula_visitor.h"

common/symbolic_generic_polynomial.cc

@@ -0,0 +1,211 @@
+// NOLINTNEXTLINE(build/include): Its header file is included in symbolic.h.
+#include <map>
+#include <sstream>
+#include <stdexcept>
+#include <utility>
+
+#include "drake/common/symbolic.h"
+#define DRAKE_COMMON_SYMBOLIC_DETAIL_HEADER
+#include "drake/common/symbolic_expression_cell.h"
+#undef DRAKE_COMMON_SYMBOLIC_DETAIL_HEADER
+
+using std::map;
+using std::ostringstream;
+using std::pair;
+using std::runtime_error;
+
+namespace drake {
+namespace symbolic {
+namespace {
+// Helper function to add coeff * m to a map (BasisElement→ Expression).
+// Used to implement DecomposePolynomialVisitor::VisitAddition and
+// GenericPolynomial::Add.
+template <typename BasisElement>
+void DoAddProduct(
+    const Expression& coeff, const BasisElement& basis_element,
+    typename GenericPolynomial<BasisElement>::MapType* const map) {
+  if (is_zero(coeff)) {
+    return;
+  }
+  auto it = map->find(basis_element);
+  if (it != map->end()) {
+    // basis_element ∈ dom(map)
+    Expression& existing_coeff = it->second;
+    // Note that `.Expand()` is needed in the following line. For example,
+    // consider the following case:
+    //     c1 := (a + b)²
+    //     c2 := - (a² + 2ab + b²)
+    // Without expanding the terms, we have `is_zero(c1 + c2) = false` while
+    // it's clear that c1 + c2 is a zero polynomial. Using `Expand()` help us
+    // identify those cases.
+    if (is_zero(existing_coeff.Expand() + coeff.Expand())) {
+      map->erase(it);
+    } else {
+      existing_coeff += coeff;
+    }
+  } else {
+    // basis_element ∉ dom(map)
+    map->emplace_hint(it, basis_element, coeff);
+  }
+}
+
+template <typename BasisElement>
+Variables GetIndeterminates(
+    const typename GenericPolynomial<BasisElement>::MapType& m) {
+  Variables vars;
+  for (const pair<const BasisElement, Expression>& p : m) {
+    const BasisElement& m_i{p.first};
+    vars += m_i.GetVariables();
+  }
+  return vars;
+}
+
+template <typename BasisElement>
+Variables GetDecisionVariables(
+    const typename GenericPolynomial<BasisElement>::MapType& m) {
+  Variables vars;
+  for (const pair<const BasisElement, Expression>& p : m) {
+    const Expression& e_i{p.second};
+    vars += e_i.GetVariables();
+  }
+  return vars;
+}
+}  // namespace
+
+template <typename BasisElement>
+GenericPolynomial<BasisElement>::GenericPolynomial(MapType init)
+    : basis_element_to_coefficient_map_{move(init)},
+      indeterminates_{
+          GetIndeterminates<BasisElement>(basis_element_to_coefficient_map_)},
+      decision_variables_{GetDecisionVariables<BasisElement>(
+          basis_element_to_coefficient_map_)} {
+  DRAKE_ASSERT_VOID(CheckInvariant());
+}
+
+template <typename BasisElement>
+void GenericPolynomial<BasisElement>::CheckInvariant() const {
+  // TODO(hongkai.dai and soonho.kong): improves the computation time of
+  // CheckInvariant(). See github issue
+  // https://github.com/RobotLocomotion/drake/issues/10229
+  Variables vars{intersect(decision_variables(), indeterminates())};
+  if (!vars.empty()) {
+    ostringstream oss;
+    oss << "Polynomial " << *this
+        << " does not satisfy the invariant because the following variable(s) "
+           "are used as decision variables and indeterminates at the same "
+           "time:\n"
+        << vars << ".";
+    throw runtime_error(oss.str());
+  }
+}
+
+template <typename BasisElement>
+GenericPolynomial<BasisElement>::GenericPolynomial(const BasisElement& m)
+    : basis_element_to_coefficient_map_{{m, 1}},
+      indeterminates_{m.GetVariables()},
+      decision_variables_{} {
+  // No need to call CheckInvariant() because the following should hold.
+  DRAKE_ASSERT(decision_variables().empty());
+}
+
+template <typename BasisElement>
+int GenericPolynomial<BasisElement>::Degree(const Variable& v) const {
+  int degree{0};
+  for (const pair<const BasisElement, Expression>& p :
+       basis_element_to_coefficient_map_) {
+    degree = std::max(degree, p.first.degree(v));
+  }
+  return degree;
+}
+
+template <typename BasisElement>
+int GenericPolynomial<BasisElement>::TotalDegree() const {
+  int degree{0};
+  for (const pair<const BasisElement, Expression>& p :
+       basis_element_to_coefficient_map_) {
+    degree = std::max(degree, p.first.total_degree());
+  }
+  return degree;
+}
+
+template <typename BasisElement>
+GenericPolynomial<BasisElement> GenericPolynomial<BasisElement>::Differentiate(
+    const Variable& x) const {
+  if (this->indeterminates_.include(x)) {
+    // x is an indeterminate.
+    GenericPolynomial<BasisElement>::MapType map;
+    for (const auto& [basis_element, coeff] :
+         this->basis_element_to_coefficient_map_) {
+      // Take the derivative of basis_element m as dm/dx = sum_{key}
+      // basis_element_gradient[key] * key.
+      const std::map<BasisElement, double> basis_element_gradient =
+          basis_element.Differentiate(x);
+
+      for (const auto& p : basis_element_gradient) {
+        DoAddProduct(coeff * p.second, p.first, &map);
+      }
+    }
+    return GenericPolynomial<BasisElement>(map);
+  } else if (decision_variables_.include(x)) {
+    // x is a decision variable.
+    GenericPolynomial<BasisElement>::MapType map;
+    for (const auto& [basis_element, coeff] :
+         basis_element_to_coefficient_map_) {
+      DoAddProduct(coeff.Differentiate(x), basis_element, &map);
+    }
+    return GenericPolynomial<BasisElement>(map);
+  } else {
+    // The variable `x` does not appear in this polynomial. Returns zero
+    // polynomial.
+    return GenericPolynomial<BasisElement>();
+  }
+}
+
+namespace {
+template <typename BasisElement>
+bool GenericPolynomialEqual(const GenericPolynomial<BasisElement>& p1,
+                            const GenericPolynomial<BasisElement>& p2,
+                            bool do_expansion) {
+  const typename GenericPolynomial<BasisElement>::MapType& map1{
+      p1.basis_element_to_coefficient_map()};
+  const typename GenericPolynomial<BasisElement>::MapType& map2{
+      p2.basis_element_to_coefficient_map()};
+  if (map1.size() != map2.size()) {
+    return false;
+  }
+  // Since both map1 and map2 are ordered map, we can compare them one by one in
+  // an ordered manner.
+  auto it1 = map1.begin();
+  auto it2 = map2.begin();
+  while (it1 != map1.end()) {
+    if (it1->first != (it2->first)) {
+      return false;
+    }
+    const Expression& e1{it1->second};
+    const Expression& e2{it2->second};
+    if (do_expansion) {
+      if (!e1.Expand().EqualTo(e2.Expand())) {
+        return false;
+      }
+    } else {
+      if (!e1.EqualTo(e2)) {
+        return false;
+      }
+    }
+    it1++;
+    it2++;
+  }
+  return true;
+}
+}  // namespace
+
+template <typename BasisElement>
+bool GenericPolynomial<BasisElement>::EqualTo(
+    const GenericPolynomial<BasisElement>& other) const {
+  return GenericPolynomialEqual<BasisElement>(*this, other, false);
+}
+
+template class GenericPolynomial<MonomialBasisElement>;
+template class GenericPolynomial<ChebyshevBasisElement>;
+}  // namespace symbolic
+}  // namespace drake

common/symbolic_generic_polynomial.h

@@ -0,0 +1,128 @@
+#pragma once
+
+#ifndef DRAKE_COMMON_SYMBOLIC_HEADER
+#error Do not directly include this file. Include "drake/common/symbolic.h".
+#endif
+
+#include <map>
+#include <ostream>
+
+#include <Eigen/Core>
+
+#include "drake/common/drake_copyable.h"
+#include "drake/common/symbolic.h"
+
+namespace drake {
+namespace symbolic {
+/**
+ * Represents symbolic generic polynomials using a given basis (for example,
+ * monomial basis, Chebyshev basis, etc). A generic symbolic polynomial keeps a
+ * mapping from a basis element of indeterminates to its coefficient in a
+ * symbolic expression. A generic polynomial `p` has to satisfy an invariant
+ * such that `p.decision_variables() ∩ p.indeterminates() = ∅`. We have
+ * CheckInvariant() method to check the invariant.
+ *
+ * Note that arithmetic operations (+,-,*) between a Polynomial and a Variable
+ * are not provided. The problem is that Variable class has no intrinsic
+ * information if a variable is a decision variable or an indeterminate while we
+ * need this information to perform arithmetic operations over Polynomials.
+ * We provide two instantiations of this template
+ * - BasisElement = MonomialBasisElement
+ * - BasisElement = ChebyshevBasisElement
+ * @tparam BasisElement Must be a subclass of PolynomialBasisElement.
+ */
+template <typename BasisElement>
+class GenericPolynomial {
+ public:
+  static_assert(
+      std::is_base_of<PolynomialBasisElement, BasisElement>::value,
+      "BasisElement should be a derived class of PolynomialBasisElement");
+  /** Type of mapping from basis element to coefficient */
+  using MapType = std::map<BasisElement, Expression>;
+
+  /** Constructs a zero polynomial. */
+  GenericPolynomial() = default;
+
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(GenericPolynomial)
+
+  /** Constructs a default value. This overload is used by Eigen when
+   * EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled.
+   */
+  explicit GenericPolynomial(std::nullptr_t)
+      : GenericPolynomial<BasisElement>() {}
+
+  /** Constructs a generic polynomial from a map, basis_element → coefficient.
+   * For example
+   * GenericPolynomial<MonomialBasiElement>({{MonomialBasisElement(x, 2), a},
+   * {MonomialBasisElement(x, 3), a+b}}) constructs a polynomial ax²+(a+b)x³.*/
+  explicit GenericPolynomial(MapType init);
+
+  /** Constructs a generic polynomial from a single basis element @p m. Note
+   * that all variables in `m` are considered as indeterminates. Namely the
+   * constructed generic polynomial contains the map with a single key `m`, with
+   * the coefficient being 1.
+   */
+  // Note that this implicit conversion is desirable to have a dot product of
+  // two Eigen::Vector<BasisElement>s return a GenericPolynomial<BasisElement>.
+  // NOLINTNEXTLINE(runtime/explicit)
+  GenericPolynomial(const BasisElement& m);
+
+  /** Returns the indeterminates of this generic polynomial. */
+  const Variables& indeterminates() const { return indeterminates_; }
+
+  /** Returns the decision variables of this generic polynomial. */
+  const Variables& decision_variables() const { return decision_variables_; }
+
+  /** Returns the map from each basis element to its coefficient. */
+  const MapType& basis_element_to_coefficient_map() const {
+    return basis_element_to_coefficient_map_;
+  }
+
+  /** Returns the highest degree of this generic polynomial in an indeterminate
+   * @p v. */
+  int Degree(const Variable& v) const;
+
+  /** Returns the total degree of this generic polynomial. */
+  int TotalDegree() const;
+
+  /**
+   * Differentiates this generic polynomial with respect to the variable @p x.
+   * Note that a variable @p x can be either a decision variable or an
+   * indeterminate.
+   */
+  GenericPolynomial<BasisElement> Differentiate(const Variable& x) const;
+
+  /** Returns true if this generic polynomial and @p p are structurally equal.
+   */
+  bool EqualTo(const GenericPolynomial<BasisElement>& p) const;
+
+ private:
+  // Throws std::runtime_error if there is a variable appeared in both of
+  // decision_variables() and indeterminates().
+  void CheckInvariant() const;
+
+  MapType basis_element_to_coefficient_map_;
+  Variables indeterminates_;
+  Variables decision_variables_;
+};
+
+template <typename BasisElement>
+std::ostream& operator<<(std::ostream& os,
+                         const GenericPolynomial<BasisElement>& p) {
+  const typename GenericPolynomial<BasisElement>::MapType& map{
+      p.basis_element_to_coefficient_map()};
+  if (map.empty()) {
+    return os << 0;
+  }
+  auto it = map.begin();
+  os << it->second << "*" << it->first;
+  for (++it; it != map.end(); ++it) {
+    os << " + " << it->second << "*" << it->first;
+  }
+  return os;
+}
+
+extern template class GenericPolynomial<MonomialBasisElement>;
+extern template class GenericPolynomial<ChebyshevBasisElement>;
+}  // namespace symbolic
+}  // namespace drake

common/symbolic_polynomial.h

@@ -63,6 +63,11 @@ struct CompareMonomial {
 /// are not provided. The problem is that Variable class has no intrinsic
 /// information if a variable is a decision variable or an indeterminate while
 /// we need this information to perform arithmetic operations over Polynomials.
+// TODO(hongkai.dai) when symbolic::GenericPolynomial is ready, we will
+// deprecate symbolic::Polynomial class, and create an alias using
+// symbolic::Polynomial=symbolic::GenericPolynomial<MonomialBasisElement>;
+// We will copy the unit tests in symbolic_polynomial_test.cc to
+// symbolic_generic_polynomial_test.cc
 class Polynomial {
  public:
   using MapType = std::map<Monomial, Expression, internal::CompareMonomial>;