diff --git a/common/symbolic_polynomial.cc b/common/symbolic_polynomial.cc index abbc040ad3f1..f13601e95a75 100644 --- a/common/symbolic_polynomial.cc +++ b/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) { diff --git a/common/symbolic_polynomial.h b/common/symbolic_polynomial.h index e2fa6180fa02..333aa2150e0c 100644 --- a/common/symbolic_polynomial.h +++ b/common/symbolic_polynomial.h @@ -5,9 +5,11 @@ #warning Do not directly include this file. Include "drake/common/symbolic.h". #endif +#include #include +#include #include -#include +#include #include @@ -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& p1, + const std::pair& 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; + using MapType = std::map; /// Constructs a zero polynomial. Polynomial() = default; diff --git a/common/test/symbolic_polynomial_test.cc b/common/test/symbolic_polynomial_test.cc index f6d79f2f07ad..a76c05468ea9 100644 --- a/common/test/symbolic_polynomial_test.cc +++ b/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