Implements symbolic::Polynomial::Roots()

bindings/pydrake/symbolic_py.cc

@@ -830,6 +830,7 @@ PYBIND11_MODULE(symbolic, m) {
           doc.Polynomial.RemoveTermsWithSmallCoefficients.doc)
       .def("IsEven", &Polynomial::IsEven, doc.Polynomial.IsEven.doc)
       .def("IsOdd", &Polynomial::IsOdd, doc.Polynomial.IsOdd.doc)
+      .def("Roots", &Polynomial::Roots, doc.Polynomial.Roots.doc)
       .def("CoefficientsAlmostEqual", &Polynomial::CoefficientsAlmostEqual,
           py::arg("p"), py::arg("tolerance"),
           doc.Polynomial.CoefficientsAlmostEqual.doc)

bindings/pydrake/test/symbolic_test.py

@@ -1425,6 +1425,13 @@ def test_even_odd(self):
         self.assertTrue(p.IsEven())
         self.assertTrue(p.IsOdd())
 
+    def test_roots(self):
+        p = sym.Polynomial(x**4 - 1)
+        roots = p.Roots()
+        numpy_compare.assert_allclose(np.sort_complex(roots), [-1, -1j, 1j, 1],
+                                      rtol=1e-14,
+                                      atol=1e-14)
+
     def test_comparison(self):
         p = sym.Polynomial()
         numpy_compare.assert_equal(p, p)

common/symbolic/BUILD.bazel

@@ -209,6 +209,7 @@ drake_cc_googletest(
     deps = [
         ":monomial_util",
         ":polynomial",
+        "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_no_throw",
         "//common/test_utilities:expect_throws_message",
         "//common/test_utilities:symbolic_test_util",

common/symbolic/polynomial.cc

@@ -1054,6 +1054,43 @@ bool Polynomial::IsOdd() const {
   return IsEvenOrOdd(*this, false /* check_even=false*/);
 }
 
+Eigen::VectorXcd Polynomial::Roots() const {
+  if (indeterminates().size() != 1) {
+    throw runtime_error(fmt::format(
+        "{} is not a univariate polynomial; it has indeterminates {}.", *this,
+        indeterminates()));
+  }
+
+  // We find the roots by computing the eigenvalues of the companion matrix.
+  // See https://en.wikipedia.org/wiki/Polynomial_root-finding_algorithms and
+  // https://www.mathworks.com/help/matlab/ref/roots.html.
+
+  const int degree = TotalDegree();
+
+  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(degree, degree);
+  for (int i = 0; i < degree - 1; ++i) {
+    C(i + 1, i) = 1;
+  }
+  double leading_coefficient = 0;
+  for (const auto& [monomial, coeff] : monomial_to_coefficient_map()) {
+    if (!is_constant(coeff)) {
+      throw runtime_error(fmt::format(
+          "Polynomial::Roots() only supports polynomials with constant "
+          "coefficients. This polynomial has coefficient {} for the "
+          "monomial {}.",
+          coeff, monomial));
+    }
+    const int power = monomial.total_degree();
+    if (power == degree) {
+      leading_coefficient = get_constant_value(coeff);
+    } else {
+      C(0, degree - power - 1) = -get_constant_value(coeff);
+    }
+  }
+  C.row(0) /= leading_coefficient;
+  return C.eigenvalues();
+}
+
 void Polynomial::CheckInvariant() const {
   // TODO(hongkai.dai and soonho.kong): improves the computation time of
   // CheckInvariant(). See github issue

common/symbolic/polynomial.h

@@ -344,6 +344,14 @@ class Polynomial {
   /// Note that this is different from the p.TotalDegree() being an odd number.
   [[nodiscard]] bool IsOdd() const;
 
+  /// Returns the roots of a _univariate_ polynomial with constant coefficients
+  /// as a column vector. There is no specific guarantee on the order of the
+  /// returned roots.
+  ///
+  /// @throws std::exception if `this` is not univariate with constant
+  /// coefficients.
+  [[nodiscard]] Eigen::VectorXcd Roots() const;
+
   Polynomial& operator+=(const Polynomial& p);
   Polynomial& operator+=(const Monomial& m);
   Polynomial& operator+=(double c);

common/symbolic/test/polynomial_test.cc

@@ -7,6 +7,7 @@
 #include <gtest/gtest.h>
 
 #include "drake/common/symbolic/monomial_util.h"
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
 #include "drake/common/test_utilities/expect_no_throw.h"
 #include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/common/test_utilities/symbolic_test_util.h"
@@ -1493,6 +1494,59 @@ TEST_F(SymbolicPolynomialTest, IsEvenOdd) {
   EXPECT_TRUE(p.IsOdd());
 }
 
+TEST_F(SymbolicPolynomialTest, Roots) {
+  symbolic::Polynomial p{0};
+  DRAKE_EXPECT_THROWS_MESSAGE(p.Roots(), ".* is not a univariate polynomial.*");
+
+  p = symbolic::Polynomial{xy_};
+  DRAKE_EXPECT_THROWS_MESSAGE(p.Roots(), ".* is not a univariate polynomial.*");
+
+  p = symbolic::Polynomial{xy_, {var_x_}};
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      p.Roots(), ".* only supports polynomials with constant coefficients.*");
+
+  p = symbolic::Polynomial{x_ - 1.23};
+  Eigen::VectorXcd roots = p.Roots();
+  EXPECT_TRUE(CompareMatrices(roots.real(), Vector1d{1.23}, 1e-14));
+  EXPECT_TRUE(CompareMatrices(roots.imag(), Vector1d::Zero(), 1e-14));
+
+  // Note: the order of the roots is not guaranteed. Sort them here by real,
+  // then imaginary.
+  auto sorted = [](const Eigen::VectorXcd& v) {
+    Eigen::VectorXcd v_sorted = v;
+    std::sort(v_sorted.data(), v_sorted.data() + v_sorted.size(),
+              [](const std::complex<double>& a, const std::complex<double>& b) {
+                if (a.real() == b.real()) {
+                  return a.imag() < b.imag();
+                }
+                return a.real() < b.real();
+              });
+    return v_sorted;
+  };
+
+  // Include a repeated root.
+  p = symbolic::Polynomial{(x_ - 1.23) * (x_ - 4.56) * (x_ - 4.56)};
+  roots = sorted(p.Roots());
+  EXPECT_TRUE(
+      CompareMatrices(roots.real(), Eigen::Vector3d{1.23, 4.56, 4.56}, 1e-7));
+  EXPECT_TRUE(CompareMatrices(roots.imag(), Eigen::Vector3d::Zero(), 1e-7));
+
+  // Complex roots.  x^4 - 1 has roots {-1, -i, i, 1}.
+  p = symbolic::Polynomial{pow(x_, 4) - 1};
+  roots = sorted(p.Roots());
+  EXPECT_TRUE(
+      CompareMatrices(roots.real(), Eigen::Vector4d{-1, 0, 0, 1}, 1e-7));
+  EXPECT_TRUE(
+      CompareMatrices(roots.imag(), Eigen::Vector4d{0, -1, 1, 0}, 1e-7));
+
+  // Leading coefficient is not 1.
+  p = symbolic::Polynomial{(2.1 * x_ - 1.23) * (x_ - 4.56)};
+  roots = sorted(p.Roots());
+  EXPECT_TRUE(
+      CompareMatrices(roots.real(), Eigen::Vector2d{1.23 / 2.1, 4.56}, 1e-7));
+  EXPECT_TRUE(CompareMatrices(roots.imag(), Eigen::Vector2d::Zero(), 1e-7));
+}
+
 TEST_F(SymbolicPolynomialTest, Expand) {
   // p1 is already expanded.
   const symbolic::Polynomial p1{