Use std::map for symbolic::Polynomial::MapType (RobotLocomotion#11643)

common/symbolic_polynomial.cc

@@ -655,7 +655,6 @@ Polynomial Polynomial::RemoveTermsWithSmallCoefficients(
     double coefficient_tol) const {
   DRAKE_DEMAND(coefficient_tol > 0);
   MapType cleaned_polynomial{};
-  cleaned_polynomial.reserve(monomial_to_coefficient_map_.size());
   for (const auto& term : monomial_to_coefficient_map_) {
     if (is_constant(term.second) &&
         std::abs(get_constant_value(term.second)) <= coefficient_tol) {

common/symbolic_polynomial.h

@@ -5,9 +5,11 @@
 #warning Do not directly include this file. Include "drake/common/symbolic.h".
 #endif
 
+#include <algorithm>
 #include <functional>
+#include <map>
 #include <ostream>
-#include <unordered_map>
+#include <utility>
 
 #include <Eigen/Core>
 
@@ -16,6 +18,40 @@
 
 namespace drake {
 namespace symbolic {
+namespace internal {
+// Compares two monomials using the lexicographic order. It is used in
+// symbolic::Polynomial::MapType. See
+// https://en.wikipedia.org/wiki/Monomial_order for different monomial orders.
+struct CompareMonomial {
+  bool operator()(const Monomial& m1, const Monomial& m2) const {
+    const auto& powers1 = m1.get_powers();
+    const auto& powers2 = m2.get_powers();
+    return std::lexicographical_compare(
+        powers1.begin(), powers1.end(), powers2.begin(), powers2.end(),
+        [](const std::pair<const Variable, int>& p1,
+           const std::pair<const Variable, int>& p2) {
+          const Variable& v1{p1.first};
+          const int i1{p1.second};
+          const Variable& v2{p2.first};
+          const int i2{p2.second};
+          if (v1.less(v2)) {
+            // m2 does not have the variable v1 explicitly, so we treat it as if
+            // it has (v1)⁰. That is, we need "return i1 < 0", but i1 should be
+            // positive, so this case always returns false.
+            return false;
+          }
+          if (v2.less(v1)) {
+            // m1 does not have the variable v2 explicitly, so we treat it as
+            // if it has (v2)⁰. That is, we need "return 0 < i2", but i2 should
+            // be positive, so it always returns true.
+            return true;
+          }
+          return i1 < i2;
+        });
+  }
+};
+}  // namespace internal
+
 /// Represents symbolic polynomials. A symbolic polynomial keeps a mapping from
 /// a monomial of indeterminates to its coefficient in a symbolic expression.
 ///
@@ -29,7 +65,7 @@ namespace symbolic {
 /// we need this information to perform arithmetic operations over Polynomials.
 class Polynomial {
  public:
-  using MapType = std::unordered_map<Monomial, Expression>;
+  using MapType = std::map<Monomial, Expression, internal::CompareMonomial>;
 
   /// Constructs a zero polynomial.
   Polynomial() = default;

common/test/symbolic_polynomial_test.cc

@@ -839,6 +839,73 @@ TEST_F(SymbolicPolynomialTest, EqualToAfterExpansion) {
   // p1 * p2 is not equal to p2 * p3 after expansion.
   EXPECT_PRED2(test::PolyNotEqualAfterExpansion, p1 * p2, p2 * p3);
 }
+
+// Checks if internal::CompareMonomial implements the lexicographical order.
+TEST_F(SymbolicPolynomialTest, InternalCompareMonomial) {
+  // clang-format off
+  const Monomial m1{{{var_x_, 1},                         }};
+  const Monomial m2{{{var_x_, 1},              {var_z_, 2}}};
+  const Monomial m3{{{var_x_, 1}, {var_y_, 1}             }};
+  const Monomial m4{{{var_x_, 1}, {var_y_, 1}, {var_z_, 1}}};
+  const Monomial m5{{{var_x_, 1}, {var_y_, 1}, {var_z_, 2}}};
+  const Monomial m6{{{var_x_, 1}, {var_y_, 2}, {var_z_, 2}}};
+  const Monomial m7{{{var_x_, 1}, {var_y_, 3}, {var_z_, 1}}};
+  const Monomial m8{{{var_x_, 2},                         }};
+  const Monomial m9{{{var_x_, 2},              {var_z_, 1}}};
+  // clang-format on
+
+  EXPECT_TRUE(internal::CompareMonomial()(m1, m2));
+  EXPECT_TRUE(internal::CompareMonomial()(m2, m3));
+  EXPECT_TRUE(internal::CompareMonomial()(m3, m4));
+  EXPECT_TRUE(internal::CompareMonomial()(m4, m5));
+  EXPECT_TRUE(internal::CompareMonomial()(m5, m6));
+  EXPECT_TRUE(internal::CompareMonomial()(m6, m7));
+  EXPECT_TRUE(internal::CompareMonomial()(m7, m8));
+  EXPECT_TRUE(internal::CompareMonomial()(m8, m9));
+
+  EXPECT_FALSE(internal::CompareMonomial()(m2, m1));
+  EXPECT_FALSE(internal::CompareMonomial()(m3, m2));
+  EXPECT_FALSE(internal::CompareMonomial()(m4, m3));
+  EXPECT_FALSE(internal::CompareMonomial()(m5, m4));
+  EXPECT_FALSE(internal::CompareMonomial()(m6, m5));
+  EXPECT_FALSE(internal::CompareMonomial()(m7, m6));
+  EXPECT_FALSE(internal::CompareMonomial()(m8, m7));
+  EXPECT_FALSE(internal::CompareMonomial()(m9, m8));
+}
+
+TEST_F(SymbolicPolynomialTest, DeterministicTraversal) {
+  // Using the following monomials, we construct two polynomials; one by summing
+  // up from top to bottom and another by summing up from bottom to top. The two
+  // polynomials should be the same mathematically. We check that the traversal
+  // operations over the two polynomials give the same sequences as well. See
+  // https://github.com/RobotLocomotion/drake/issues/11023#issuecomment-499948333
+  // for details.
+
+  const Monomial m1{{{var_x_, 1}}};
+  const Monomial m2{{{var_x_, 1}, {var_y_, 1}}};
+  const Monomial m3{{{var_x_, 1}, {var_y_, 1}, {var_z_, 1}}};
+  const Monomial m4{{{var_x_, 1}, {var_y_, 1}, {var_z_, 2}}};
+
+  const Polynomial p1{m1 + (m2 + (m3 + m4))};
+  const Polynomial p2{m4 + (m3 + (m2 + m1))};
+
+  const Polynomial::MapType& map1{p1.monomial_to_coefficient_map()};
+  const Polynomial::MapType& map2{p2.monomial_to_coefficient_map()};
+
+  EXPECT_EQ(map1.size(), map2.size());
+
+  auto it1 = map1.begin();
+  auto it2 = map2.begin();
+
+  for (; it1 != map1.end(); ++it1, ++it2) {
+    const Monomial& m_1{it1->first};
+    const Monomial& m_2{it2->first};
+    const Expression& e_1{it1->second};
+    const Expression& e_2{it2->second};
+    EXPECT_TRUE(m_1 == m_2);
+    EXPECT_TRUE(e_1.EqualTo(e_2));
+  }
+}
 }  // namespace
 }  // namespace symbolic
 }  // namespace drake