From a6eebc949bd24733415da0c73d4d7ad64132c43b Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Mon, 11 Sep 2023 10:05:04 -0400 Subject: [PATCH] Implements symbolic::Polynomial::Roots() (#20161) For finding the roots of univariate polynomials. Towards #20159. --- bindings/pydrake/symbolic_py.cc | 1 + bindings/pydrake/test/symbolic_test.py | 7 ++++ common/symbolic/BUILD.bazel | 1 + common/symbolic/polynomial.cc | 37 +++++++++++++++++ common/symbolic/polynomial.h | 8 ++++ common/symbolic/test/polynomial_test.cc | 54 +++++++++++++++++++++++++ 6 files changed, 108 insertions(+) diff --git a/bindings/pydrake/symbolic_py.cc b/bindings/pydrake/symbolic_py.cc index 78dd739b23cd..3b796ccc7616 100644 --- a/bindings/pydrake/symbolic_py.cc +++ b/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) diff --git a/bindings/pydrake/test/symbolic_test.py b/bindings/pydrake/test/symbolic_test.py index 9629eb42b9d7..855d9b0a2fcc 100644 --- a/bindings/pydrake/test/symbolic_test.py +++ b/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) diff --git a/common/symbolic/BUILD.bazel b/common/symbolic/BUILD.bazel index 8a66ae7ed4ca..b693a89f4b22 100644 --- a/common/symbolic/BUILD.bazel +++ b/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", diff --git a/common/symbolic/polynomial.cc b/common/symbolic/polynomial.cc index bc51df8f0d96..ff3aecd147f3 100644 --- a/common/symbolic/polynomial.cc +++ b/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 diff --git a/common/symbolic/polynomial.h b/common/symbolic/polynomial.h index 6434400a27d7..14862f7f5373 100644 --- a/common/symbolic/polynomial.h +++ b/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); diff --git a/common/symbolic/test/polynomial_test.cc b/common/symbolic/test/polynomial_test.cc index 7e1dced85984..0d81c01f26d1 100644 --- a/common/symbolic/test/polynomial_test.cc +++ b/common/symbolic/test/polynomial_test.cc @@ -7,6 +7,7 @@ #include #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& a, const std::complex& 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{