Represents symbolic polynomials.
A symbolic polynomial keeps a mapping from a monomial of indeterminates to its coefficient in a symbolic expression.
A 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.
#include <drake/common/symbolic_polynomial.h>
Public Types | |
using | MapType = std::map< Monomial, Expression, internal::CompareMonomial > |
Public Member Functions | |
Polynomial ()=default | |
Constructs a zero polynomial. More... | |
Polynomial (std::nullptr_t) | |
Constructs a default value. More... | |
Polynomial (MapType map) | |
Constructs a polynomial from a map, Monomial → Expression. More... | |
Polynomial (const Monomial &m) | |
Constructs a polynomial from a monomial m . More... | |
Polynomial (const Expression &e) | |
Constructs a polynomial from an expression e . More... | |
Polynomial (const Expression &e, Variables indeterminates) | |
Constructs a polynomial from an expression e by decomposing it with respect to indeterminates . More... | |
const Variables & | indeterminates () const |
Returns the indeterminates of this polynomial. More... | |
const Variables & | decision_variables () const |
Returns the decision variables of this polynomial. More... | |
void | SetIndeterminates (const Variables &new_indeterminates) |
Sets the indeterminates to new_indeterminates . More... | |
int | Degree (const Variable &v) const |
Returns the highest degree of this polynomial in a variable v . More... | |
int | TotalDegree () const |
Returns the total degree of this polynomial. More... | |
const MapType & | monomial_to_coefficient_map () const |
Returns the mapping from a Monomial to its corresponding coefficient of this polynomial. More... | |
Expression | ToExpression () const |
Returns an equivalent symbolic expression of this polynomial. More... | |
Polynomial | Differentiate (const Variable &x) const |
Differentiates this polynomial with respect to the variable x . More... | |
template<typename Derived > | |
Eigen::Matrix< Polynomial, 1, Derived::RowsAtCompileTime > | Jacobian (const Eigen::MatrixBase< Derived > &vars) const |
Computes the Jacobian matrix J of the polynomial with respect to vars . More... | |
Polynomial | Integrate (const Variable &x) const |
Integrates this polynomial with respect to an indeterminate x . More... | |
Polynomial | Integrate (const Variable &x, double a, double b) const |
Computes the definite integrate of this polynomial with respect to the indeterminate x over the domain [a, b]. More... | |
double | Evaluate (const Environment &env) const |
Evaluates this polynomial under a given environment env . More... | |
Polynomial | EvaluatePartial (const Environment &env) const |
Partially evaluates this polynomial using an environment env . More... | |
Polynomial | EvaluatePartial (const Variable &var, double c) const |
Partially evaluates this polynomial by substituting var with c . More... | |
Eigen::VectorXd | EvaluateIndeterminates (const Eigen::Ref< const VectorX< symbolic::Variable >> &indeterminates, const Eigen::Ref< const Eigen::MatrixXd > &indeterminates_values) const |
Evaluates the polynomial at a batch of indeterminates values. More... | |
void | EvaluateWithAffineCoefficients (const Eigen::Ref< const VectorX< symbolic::Variable >> &indeterminates, const Eigen::Ref< const Eigen::MatrixXd > &indeterminates_values, Eigen::MatrixXd *A, VectorX< symbolic::Variable > *decision_variables, Eigen::VectorXd *b) const |
Evaluates the polynomial at a batch of indeterminate values. More... | |
Polynomial & | AddProduct (const Expression &coeff, const Monomial &m) |
Adds coeff * m to this polynomial. More... | |
Polynomial | Expand () const |
Expands each coefficient expression and returns the expanded polynomial. More... | |
Polynomial | RemoveTermsWithSmallCoefficients (double coefficient_tol) const |
Removes the terms whose absolute value of the coefficients are smaller than or equal to coefficient_tol For example, if the polynomial is 2x² + 3xy + 10⁻⁴x - 10⁻⁵, then after calling RemoveTermsWithSmallCoefficients(1e-3), the returned polynomial becomes 2x² + 3xy. More... | |
bool | IsEven () const |
Returns true if the polynomial is even, namely p(x) = p(-x). More... | |
bool | IsOdd () const |
Returns true if the polynomial is odd, namely p(x) = -p(-x). More... | |
Polynomial & | operator+= (const Polynomial &p) |
Polynomial & | operator+= (const Monomial &m) |
Polynomial & | operator+= (double c) |
Polynomial & | operator+= (const Variable &v) |
Polynomial & | operator-= (const Polynomial &p) |
Polynomial & | operator-= (const Monomial &m) |
Polynomial & | operator-= (double c) |
Polynomial & | operator-= (const Variable &v) |
Polynomial & | operator *= (const Polynomial &p) |
Polynomial & | operator *= (const Monomial &m) |
Polynomial & | operator *= (double c) |
Polynomial & | operator *= (const Variable &v) |
bool | EqualTo (const Polynomial &p) const |
Returns true if this polynomial and p are structurally equal. More... | |
bool | EqualToAfterExpansion (const Polynomial &p) const |
(Deprecated.) More... | |
bool | CoefficientsAlmostEqual (const Polynomial &p, double tolerance) const |
Returns true if this polynomial and p are almost equal (the difference in the corresponding coefficients are all less than tolerance ), after expanding the coefficients. More... | |
Formula | operator== (const Polynomial &p) const |
Returns a symbolic formula representing the condition where this polynomial and p are the same. More... | |
Formula | operator!= (const Polynomial &p) const |
Returns a symbolic formula representing the condition where this polynomial and p are not the same. More... | |
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable | |
Polynomial (const Polynomial &)=default | |
Polynomial & | operator= (const Polynomial &)=default |
Polynomial (Polynomial &&)=default | |
Polynomial & | operator= (Polynomial &&)=default |
Friends | |
template<class HashAlgorithm > | |
void | hash_append (HashAlgorithm &hasher, const Polynomial &item) noexcept |
Implements the hash_append generic hashing concept. More... | |
Polynomial | operator/ (Polynomial p, double v) |
Returns p / v . More... | |
using MapType = std::map<Monomial, Expression, internal::CompareMonomial> |
|
default |
Constructs a zero polynomial.
|
default |
|
default |
|
explicit |
Constructs a default value.
This overload is used by Eigen when EIGEN_INITIALIZE_MATRICES_BY_ZERO is enabled.
|
explicit |
Constructs a polynomial from a map, Monomial → Expression.
Polynomial | ( | const Monomial & | m | ) |
Constructs a polynomial from a monomial m
.
Note that all variables in m
are considered as indeterminates.
|
explicit |
Constructs a polynomial from an expression e
.
Note that all variables in e
are considered as indeterminates.
std::exception | if e is not a polynomial. |
Polynomial | ( | const Expression & | e, |
Variables | indeterminates | ||
) |
Constructs a polynomial from an expression e
by decomposing it with respect to indeterminates
.
e
and the provided indeterminates
.std::exception | if e is not a polynomial in indeterminates . |
Polynomial& AddProduct | ( | const Expression & | coeff, |
const Monomial & | m | ||
) |
Adds coeff
* m
to this polynomial.
bool CoefficientsAlmostEqual | ( | const Polynomial & | p, |
double | tolerance | ||
) | const |
Returns true if this polynomial and p
are almost equal (the difference in the corresponding coefficients are all less than tolerance
), after expanding the coefficients.
const Variables& decision_variables | ( | ) | const |
Returns the decision variables of this polynomial.
Returns the highest degree of this polynomial in a variable v
.
Polynomial Differentiate | ( | const Variable & | x | ) | const |
Differentiates this polynomial with respect to the variable x
.
Note that a variable x
can be either a decision variable or an indeterminate.
bool EqualTo | ( | const Polynomial & | p | ) | const |
Returns true if this polynomial and p
are structurally equal.
bool EqualToAfterExpansion | ( | const Polynomial & | p | ) | const |
(Deprecated.)
double Evaluate | ( | const Environment & | env | ) | const |
Evaluates this polynomial under a given environment env
.
std::exception | if there is a variable in this polynomial whose assignment is not provided by env . |
Eigen::VectorXd EvaluateIndeterminates | ( | const Eigen::Ref< const VectorX< symbolic::Variable >> & | indeterminates, |
const Eigen::Ref< const Eigen::MatrixXd > & | indeterminates_values | ||
) | const |
Evaluates the polynomial at a batch of indeterminates values.
[in] | indeterminates | Must include all this->indeterminates() |
[in] | indeterminates_values | Each column of indeterminates_values stores one specific value of indeterminates . indeterminates_values.rows() == indeterminates.rows(). |
std::exception | if any coefficient in this polynomial is not a constant. |
Polynomial EvaluatePartial | ( | const Environment & | env | ) | const |
Partially evaluates this polynomial using an environment env
.
std::exception | if NaN is detected during evaluation. |
Polynomial EvaluatePartial | ( | const Variable & | var, |
double | c | ||
) | const |
Partially evaluates this polynomial by substituting var
with c
.
std::exception | if NaN is detected at any point during evaluation. |
void EvaluateWithAffineCoefficients | ( | const Eigen::Ref< const VectorX< symbolic::Variable >> & | indeterminates, |
const Eigen::Ref< const Eigen::MatrixXd > & | indeterminates_values, | ||
Eigen::MatrixXd * | A, | ||
VectorX< symbolic::Variable > * | decision_variables, | ||
Eigen::VectorXd * | b | ||
) | const |
Evaluates the polynomial at a batch of indeterminate values.
For a polynomial whose coefficients are affine expressions of decision variables, we evaluate this polynomial on a batch of indeterminate values, and return the matrix representation of the evaluated affine expressions. For example if p(x) = (a+1)x² + b*x where a, b are decision variables, if we evaluate this polynomial on x = 1 and x = 2, then p(x) = a+b+1 and 4a+2b+4 respectively. We return the evaluation result as A * decision_variables + b, where A.row(i) * decision_variables + b(i) is the evaluation of the polynomial on indeterminates_values.col(i).
[in] | indeterminates | Must include all this->indeterminates() |
[in] | indeterminates_values | A matrix representing a batch of values. Each column of indeterminates_values stores one specific value of indeterminates , where indeterminates_values.rows() == indeterminates.rows(). |
[out] | A | The coefficient of the evaluation results. |
[out] | decision_variables | The decision variables in the evaluation results. |
[out] | b | The constant terms in the evaluation results. |
std::exception | if the coefficients of this polynomial is not an affine expression of its decision variables. For example, the polynomial (2+sin(a)) * x² + 1 (where a is a decision variable and x is a indeterminate) doesn't have affine expression as its coefficient 2+sin(a). |
Polynomial Expand | ( | ) | const |
Expands each coefficient expression and returns the expanded polynomial.
If any coefficient is equal to 0 after expansion, then remove that term from the returned polynomial.
const Variables& indeterminates | ( | ) | const |
Returns the indeterminates of this polynomial.
Polynomial Integrate | ( | const Variable & | x | ) | const |
Integrates this polynomial with respect to an indeterminate x
.
Integration with respect to decision variables is not supported yet. If x
is not an indeterminate nor decision variable, then it will be added to the list of indeterminates.
std::exception | if x is a decision variable. |
Polynomial Integrate | ( | const Variable & | x, |
double | a, | ||
double | b | ||
) | const |
Computes the definite integrate of this polynomial with respect to the indeterminate x
over the domain [a, b].
Integration with respect to decision variables is not supported yet.
std::exception | if x is a decision variable. |
bool IsEven | ( | ) | const |
Returns true if the polynomial is even, namely p(x) = p(-x).
Meaning that the coefficient for all odd-degree monomials are 0. Returns false otherwise. Note that this is different from the p.TotalDegree() being an even number.
bool IsOdd | ( | ) | const |
Returns true if the polynomial is odd, namely p(x) = -p(-x).
Meaning that the coefficient for all even-degree monomials are 0. Returns false otherwise. Note that this is different from the p.TotalDegree() being an odd number.
Eigen::Matrix<Polynomial, 1, Derived::RowsAtCompileTime> Jacobian | ( | const Eigen::MatrixBase< Derived > & | vars | ) | const |
Computes the Jacobian matrix J of the polynomial with respect to vars
.
J(0,i) contains ∂f/∂vars(i).
const MapType& monomial_to_coefficient_map | ( | ) | const |
Returns the mapping from a Monomial to its corresponding coefficient of this polynomial.
Polynomial& operator *= | ( | const Polynomial & | p | ) |
Polynomial& operator *= | ( | const Monomial & | m | ) |
Polynomial& operator *= | ( | double | c | ) |
Polynomial& operator *= | ( | const Variable & | v | ) |
Formula operator!= | ( | const Polynomial & | p | ) | const |
Returns a symbolic formula representing the condition where this polynomial and p
are not the same.
Polynomial& operator+= | ( | const Polynomial & | p | ) |
Polynomial& operator+= | ( | const Monomial & | m | ) |
Polynomial& operator+= | ( | double | c | ) |
Polynomial& operator+= | ( | const Variable & | v | ) |
Polynomial& operator-= | ( | const Polynomial & | p | ) |
Polynomial& operator-= | ( | const Monomial & | m | ) |
Polynomial& operator-= | ( | double | c | ) |
Polynomial& operator-= | ( | const Variable & | v | ) |
|
default |
|
default |
Formula operator== | ( | const Polynomial & | p | ) | const |
Returns a symbolic formula representing the condition where this polynomial and p
are the same.
Polynomial RemoveTermsWithSmallCoefficients | ( | double | coefficient_tol | ) | const |
Removes the terms whose absolute value of the coefficients are smaller than or equal to coefficient_tol
For example, if the polynomial is 2x² + 3xy + 10⁻⁴x - 10⁻⁵, then after calling RemoveTermsWithSmallCoefficients(1e-3), the returned polynomial becomes 2x² + 3xy.
coefficient_tol | A positive scalar. |
polynomial_cleaned | A polynomial whose terms with small coefficients are removed. |
void SetIndeterminates | ( | const Variables & | new_indeterminates | ) |
Sets the indeterminates to new_indeterminates
.
Changing the indeterminates would change monomial_to_coefficient_map()
, and also potentially the degree of the polynomial. Here is an example.
Expression ToExpression | ( | ) | const |
Returns an equivalent symbolic expression of this polynomial.
int TotalDegree | ( | ) | const |
Returns the total degree of this polynomial.
|
friend |
Implements the hash_append generic hashing concept.
|
friend |
Returns p / v
.