Drake
polynomial.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <complex>
4 #include <map>
5 #include <random>
6 #include <set>
7 #include <stdexcept>
8 #include <string>
9 #include <vector>
10 
11 #include <Eigen/Core>
12 #include <unsupported/Eigen/Polynomials>
13 
14 #include "drake/common/autodiff.h"
16 
17 /** A scalar multi-variate polynomial, modeled after the msspoly in spotless.
18  *
19  * Polynomial represents a list of additive Monomials, each one of which is a
20  * product of a constant coefficient (of _CoefficientType, which by default is
21  * double) and any number of distinct Terms (variables raised to positive
22  * integer powers).
23  *
24  * Variables are identified by integer indices rather than symbolic names, but
25  * an automatic facility is provided to covert variable names up to four
26  * characters into unique integers, provided those variables are named using
27  * only lowercase letters and the "@#_." characters followed by a number. For
28  * example, valid names include "dx4" and "m_x".
29  *
30  * Monomials which have the same variables and powers may be constructed but
31  * will be automatically combined:
32  * (3 * a * b * a) + (1.5 * b * a**2)
33  * will be reduced to
34  * (4.5 * b * a**2)
35  * internally after construction.
36  *
37  * Polynomials can be added, subtracted, and multiplied. They may only be
38  * divided by scalars (of _CoefficientType) because Polynomials are not closed
39  * under division.
40  */
41 template <typename _CoefficientType = double>
42 class Polynomial {
43  public:
44  typedef _CoefficientType CoefficientType;
45  typedef unsigned int VarType;
46  /// This should be 'unsigned int' but MSVC considers a call to std::pow(...,
47  /// unsigned int) ambiguous because it won't cast unsigned int to int.
48  typedef int PowerType;
49  typedef typename Eigen::NumTraits<CoefficientType>::Real RealScalar;
50  typedef std::complex<RealScalar> RootType;
51  typedef Eigen::Matrix<RootType, Eigen::Dynamic, 1> RootsType;
52 
53  template <typename Rhs, typename Lhs>
54  struct Product {
55  typedef decltype(static_cast<Rhs>(0) * static_cast<Lhs>(0)) type;
56  };
57 
58  /// An individual variable raised to an integer power; e.g. x**2.
59  class Term {
60  public:
61  VarType var;
62  PowerType power;
63 
64  bool operator==(const Term& other) const {
65  return (var == other.var) && (power == other.power);
66  }
67 
68  /// A comparison to allow std::lexicographical_compare on this class; does
69  /// not reflect any sort of mathematical total order.
70  bool operator<(const Term& other) const {
71  return ((var < other.var) ||
72  ((var == other.var) && (power < other.power)));
73  }
74  };
75 
76  /// An additive atom of a Polynomial: The product of any number of
77  /// Terms and a coefficient.
78  class Monomial {
79  public:
80  CoefficientType coefficient;
81  std::vector<Term> terms; // a list of N variable ids
82 
83  bool operator==(const Monomial& other) const {
84  return (coefficient == other.coefficient) && (terms == other.terms);
85  }
86 
87  /// A comparison to allow std::lexicographical_compare on this class; does
88  /// not reflect any sort of mathematical total order.
89  bool operator<(const Monomial& other) const {
90  return ((coefficient < other.coefficient) ||
91  ((coefficient == other.coefficient) && (terms < other.terms)));
92  }
93 
94  int GetDegree() const;
95  int GetDegreeOf(VarType var) const;
96  bool HasSameExponents(const Monomial& other) const;
97 
98  /// Factors this by other; returns 0 iff other does not divide this.
99  Monomial Factor(const Monomial& divisor) const;
100  };
101 
102  private:
103  /// The Monomial atoms of the Polynomial.
104  std::vector<Monomial> monomials_;
105 
106  /// True iff only 0 or 1 distinct variables appear in the Polynomial.
107  bool is_univariate_;
108 
109  public:
110  /// Construct the vacuous polynomial, "0".
111  Polynomial(void) : is_univariate_(true) {}
112 
113  /// Construct a Polynomial of a single constant. e.g. "5".
114  // This is required for some Eigen operations when used in a
115  // polynomial matrix.
116  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
117  Polynomial(const CoefficientType& scalar);
118 
119  /// Construct a Polynomial consisting of a single Monomial, e.g. "5xy**3".
120  Polynomial(const CoefficientType coeff, const std::vector<Term>& terms);
121 
122  /// Construct a Polynomial from a sequence of Monomials.
123  Polynomial(typename std::vector<Monomial>::const_iterator start,
124  typename std::vector<Monomial>::const_iterator finish);
125 
126  /// Construct a polynomial consisting of a single Monomial of the variable
127  /// named varname + num.
128  explicit Polynomial(const std::string varname, const unsigned int num = 1);
129 
130  /// Construct a single Monomial of the given coefficient and variable.
131  Polynomial(const CoefficientType& coeff, const VarType& v);
132 
133  /// A legacy constructor for univariate polynomials: Takes a vector
134  /// of coefficients for the constant, x, x**2, x**3... Monomials.
135  template <typename Derived>
136  explicit Polynomial(Eigen::MatrixBase<Derived> const& coefficients) {
137  VarType v = VariableNameToId("t");
138  for (int i = 0; i < coefficients.size(); i++) {
139  Monomial m;
140  m.coefficient = coefficients(i);
141  if (i > 0) {
142  Term t;
143  t.var = v;
144  t.power = i;
145  m.terms.push_back(t);
146  }
147  monomials_.push_back(m);
148  }
149  is_univariate_ = true;
150  }
151 
152  /// Returns the number of unique Monomials (and thus the number of
153  /// coefficients) in this Polynomial.
154  int GetNumberOfCoefficients() const;
155 
156  /** Returns the highest degree of any Monomial in this Polynomial.
157  *
158  * The degree of a multivariate Monomial is the product of the degrees of
159  * each of its terms. */
160  int GetDegree() const;
161 
162  /// Returns true iff this is a sum of terms of degree 1, plus a constant.
163  bool IsAffine() const;
164 
165  /// If the polynomial is "simple" -- e.g. just a single term with
166  /// coefficient 1 -- then returns that variable; otherwise returns 0.
167  VarType GetSimpleVariable() const;
168 
169  const std::vector<Monomial>& GetMonomials() const;
170 
171  Eigen::Matrix<CoefficientType, Eigen::Dynamic, 1> GetCoefficients() const;
172 
173  /// Returns a set of all of the variables present in this Polynomial.
174  std::set<VarType> GetVariables() const;
175 
176  /** Evaluate a univariate Polynomial at a specific point.
177  *
178  * Evaluates a univariate Polynomial at the given x. Throws an
179  * exception of this Polynomial is not univariate.
180  *
181  * x may be of any type supporting the ** and + operations (which can
182  * be different from both CoefficientsType and RealScalar)
183  */
184  template <typename T>
186  const T& x) const {
187  typedef typename Product<CoefficientType, T>::type ProductType;
188 
189  if (!is_univariate_)
190  throw std::runtime_error(
191  "this method can only be used for univariate polynomials");
192  ProductType value = 0;
193  using std::pow;
194  for (typename std::vector<Monomial>::const_iterator iter =
195  monomials_.begin();
196  iter != monomials_.end(); iter++) {
197  if (iter->terms.empty())
198  value += iter->coefficient;
199  else
200  value += iter->coefficient *
201  pow(static_cast<ProductType>(x),
202  static_cast<PowerType>(iter->terms[0].power));
203  }
204  return value;
205  }
206 
207  /** Evaluate a multivariate Polynomial at a specific point.
208  *
209  * Evaluates a Polynomial with the given values for each variable. Throws
210  * std::out_of_range if the Polynomial contains variables for which values
211  * were not provided.
212  *
213  * The provided values may be of any type which is std::is_arithmetic
214  * (supporting the std::pow, *, and + operations) and need not be
215  * CoefficientsType or RealScalar)
216  */
217  template <typename T>
219  const std::map<VarType, T>& var_values) const {
220  typedef typename std::remove_const<
221  typename Product<CoefficientType, T>::type>::type ProductType;
222  ProductType value = 0;
223  for (const Monomial& monomial : monomials_) {
224  ProductType monomial_value = monomial.coefficient;
225  for (const Term& term : monomial.terms) {
226  monomial_value *= std::pow(
227  static_cast<ProductType>(var_values.at(term.var)),
228  term.power);
229  }
230  value += monomial_value;
231  }
232  return value;
233  }
234 
235  /** Specialization of EvaluateMultivariate on AutoDiffXd.
236  *
237  * Specialize EvaluateMultivariate on AutoDiffXd because Eigen autodiffs
238  * implement a confusing subset of operators and conversions that makes a
239  * strictly generic approach too confusing and unreadable.
240  *
241  * Note that it is up to the caller to ensure that all of the AutoDiffXds
242  * in var_values correctly correspond to one another, because Polynomial has
243  * no knowledge of what partial derivative terms the indices of a given
244  * AutoDiffXd correspond to.
245  */
247  const std::map<VarType, drake::AutoDiffXd>& var_values) const {
249  for (const Monomial& monomial : monomials_) {
250  drake::AutoDiffXd monomial_value(monomial.coefficient);
251  for (const Term& term : monomial.terms) {
252  monomial_value *= pow(var_values.at(term.var), term.power);
253  }
254  value += monomial_value;
255  }
256  return value;
257  }
258 
259  /** Substitute values for some but not necessarily all variables of a
260  * Polynomial.
261  *
262  * Analogous to EvaluateMultivariate, but:
263  * (1) Restricted to CoefficientType, and
264  * (2) Need not map every variable in var_values.
265  *
266  * Returns a Polynomial in which each variable in var_values has been
267  * replaced with its value and constants appropriately combined.
268  */
270  const std::map<VarType, CoefficientType>& var_values) const;
271 
272  /// Replaces all instances of variable orig with replacement.
273  void Subs(const VarType& orig, const VarType& replacement);
274 
275  /** Takes the derivative of this (univariate) Polynomial.
276  *
277  * Returns a new Polynomial that is the derivative of this one in its sole
278  * variable. Throws an exception of this Polynomial is not univariate.
279  *
280  * If derivative_order is given, takes the nth derivative of this
281  * Polynomial.
282  */
283  Polynomial Derivative(int derivative_order = 1) const;
284 
285  /** Takes the integral of this (univariate, non-constant) Polynomial.
286  *
287  * Returns a new Polynomial that is the indefinite integral of this one in
288  * its sole variable. Throws an exception of this Polynomial is not
289  * univariate, or if it has no variables.
290  *
291  * If integration_constant is given, adds that constant as the constant
292  * term (zeroth-order coefficient) of the resulting Polynomial.
293  */
294  Polynomial Integral(const CoefficientType& integration_constant = 0.0) const;
295 
296  bool operator==(const Polynomial& other) const;
297 
298  Polynomial& operator+=(const Polynomial& other);
299 
300  Polynomial& operator-=(const Polynomial& other);
301 
302  Polynomial& operator*=(const Polynomial& other);
303 
304  Polynomial& operator+=(const CoefficientType& scalar);
305 
306  Polynomial& operator-=(const CoefficientType& scalar);
307 
308  Polynomial& operator*=(const CoefficientType& scalar);
309 
310  Polynomial& operator/=(const CoefficientType& scalar);
311 
312  const Polynomial operator+(const Polynomial& other) const;
313 
314  const Polynomial operator-(const Polynomial& other) const;
315 
316  const Polynomial operator-() const;
317 
318  const Polynomial operator*(const Polynomial& other) const;
319 
320  friend const Polynomial operator+(const Polynomial& p,
321  const CoefficientType& scalar) {
322  Polynomial ret = p;
323  ret += scalar;
324  return ret;
325  }
326 
327  friend const Polynomial operator+(const CoefficientType& scalar,
328  const Polynomial& p) {
329  Polynomial ret = p;
330  ret += scalar;
331  return ret;
332  }
333 
334  friend const Polynomial operator-(const Polynomial& p,
335  const CoefficientType& scalar) {
336  Polynomial ret = p;
337  ret -= scalar;
338  return ret;
339  }
340 
341  friend const Polynomial operator-(const CoefficientType& scalar,
342  const Polynomial& p) {
343  Polynomial ret = -p;
344  ret += scalar;
345  return ret;
346  }
347 
348  friend const Polynomial operator*(const Polynomial& p,
349  const CoefficientType& scalar) {
350  Polynomial ret = p;
351  ret *= scalar;
352  return ret;
353  }
354  friend const Polynomial operator*(const CoefficientType& scalar,
355  const Polynomial& p) {
356  Polynomial ret = p;
357  ret *= scalar;
358  return ret;
359  }
360 
361  const Polynomial operator/(const CoefficientType& scalar) const;
362 
363  /// A comparison to allow std::lexicographical_compare on this class; does
364  /// not reflect any sort of mathematical total order.
365  bool operator<(const Polynomial& other) const {
366  // Just delegate to the default vector std::lexicographical_compare.
367  return monomials_ < other.monomials_;
368  }
369 
370  /** Returns the roots of this (univariate) Polynomial.
371  *
372  * Returns the roots of a univariate Polynomial as an Eigen column vector of
373  * complex numbers whose components are of the RealScalar type. Throws an
374  * exception of this Polynomial is not univariate.
375  */
376  RootsType Roots() const;
377 
378  /** Checks if a (univariate) Polynomial is approximately equal to this one.
379  *
380  * Checks that every coefficient of other is within tol of the
381  * corresponding coefficient of this Polynomial. Throws an exception if
382  * either Polynomial is not univariate.
383  */
384  bool IsApprox(const Polynomial& other, const RealScalar& tol) const;
385 
386  friend std::ostream& operator<<(std::ostream& os, const Monomial& m) {
387  // if (m.coefficient == 0) return os;
388 
389  bool print_star = false;
390  if (m.coefficient == -1) {
391  os << "-";
392  } else if (m.coefficient != 1 || m.terms.empty()) {
393  os << '(' << m.coefficient << ")";
394  print_star = true;
395  }
396 
397  for (typename std::vector<Term>::const_iterator iter = m.terms.begin();
398  iter != m.terms.end(); iter++) {
399  if (print_star)
400  os << '*';
401  else
402  print_star = true;
403  os << IdToVariableName(iter->var);
404  if (iter->power != 1) {
405  os << "^" << iter->power;
406  }
407  }
408  return os;
409  }
410 
411  friend std::ostream& operator<<(std::ostream& os, const Polynomial& poly) {
412  if (poly.monomials_.empty()) {
413  os << "0";
414  return os;
415  }
416 
417  for (typename std::vector<Monomial>::const_iterator iter =
418  poly.monomials_.begin();
419  iter != poly.monomials_.end(); iter++) {
420  os << *iter;
421  if (iter + 1 != poly.monomials_.end() && (iter + 1)->coefficient != -1)
422  os << '+';
423  }
424  return os;
425  }
426 
427  //@{
428  /** Variable name/ID conversion facility. */
429  static bool IsValidVariableName(const std::string name);
430 
431  static VarType VariableNameToId(const std::string name,
432  const unsigned int m = 1);
433 
434  static std::string IdToVariableName(const VarType id);
435  //@}
436 
437  template <typename CoefficientType>
441 
442  private:
443  /// Sorts through Monomial list and merges any that have the same powers.
444  void MakeMonomialsUnique(void);
445 };
446 
447 /** Provides power function for Polynomial. */
448 template <typename CoefficientType>
450  const Polynomial<CoefficientType>& base,
451  typename Polynomial<CoefficientType>::PowerType exponent) {
452  DRAKE_DEMAND(exponent >= 0);
453  if (exponent == 0) {
454  return Polynomial<CoefficientType>{1.0};
455  }
456  const Polynomial<CoefficientType> pow_half{pow(base, exponent / 2)};
457  if (exponent % 2 == 1) {
458  return base * pow_half * pow_half; // Odd exponent case.
459  } else {
460  return pow_half * pow_half; // Even exponent case.
461  }
462 }
463 
464 template <typename CoefficientType, int Rows, int Cols>
465 std::ostream& operator<<(
466  std::ostream& os,
467  const Eigen::Matrix<Polynomial<CoefficientType>, Rows, Cols>& poly_mat) {
468  for (int i = 0; i < poly_mat.rows(); i++) {
469  os << "[ ";
470  for (int j = 0; j < poly_mat.cols(); j++) {
471  os << poly_mat(i, j);
472  if (j < (poly_mat.cols() - 1)) os << " , ";
473  }
474  os << " ]" << std::endl;
475  }
476  return os;
477 }
478 
480 
481 /// A column vector of polynomials; used in several optimization classes.
482 typedef Eigen::Matrix<Polynomiald, Eigen::Dynamic, 1> VectorXPoly;
drake::AutoDiffXd EvaluateMultivariate(const std::map< VarType, drake::AutoDiffXd > &var_values) const
Specialization of EvaluateMultivariate on AutoDiffXd.
Definition: polynomial.h:246
void Subs(const VarType &orig, const VarType &replacement)
Replaces all instances of variable orig with replacement.
Eigen::AutoDiffScalar< Eigen::VectorXd > AutoDiffXd
An autodiff variable with a dynamic number of partials.
Definition: eigen_autodiff_types.h:25
const Polynomial operator+(const Polynomial &other) const
const std::vector< Monomial > & GetMonomials() const
vector< Monomial > monomials_
Definition: symbolic_monomial_test.cc:41
std::set< VarType > GetVariables() const
Returns a set of all of the variables present in this Polynomial.
Eigen::Matrix< Polynomiald, Eigen::Dynamic, 1 > VectorXPoly
A column vector of polynomials; used in several optimization classes.
Definition: polynomial.h:482
int GetNumberOfCoefficients() const
Returns the number of unique Monomials (and thus the number of coefficients) in this Polynomial...
bool operator<(const Polynomial &other) const
A comparison to allow std::lexicographical_compare on this class; does not reflect any sort of mathem...
Definition: polynomial.h:365
bool operator<(const Term &other) const
A comparison to allow std::lexicographical_compare on this class; does not reflect any sort of mathem...
Definition: polynomial.h:70
std::vector< Number > x
Definition: ipopt_solver.cc:150
bool operator==(const Term &other) const
Definition: polynomial.h:64
friend std::ostream & operator<<(std::ostream &os, const Polynomial &poly)
Definition: polynomial.h:411
Polynomial & operator-=(const Polynomial &other)
Polynomial< double > Polynomiald
Definition: polynomial.h:479
static bool IsValidVariableName(const std::string name)
Variable name/ID conversion facility.
int PowerType
This should be &#39;unsigned int&#39; but MSVC considers a call to std::pow(..., unsigned int) ambiguous beca...
Definition: polynomial.h:48
Polynomial Derivative(int derivative_order=1) const
Takes the derivative of this (univariate) Polynomial.
PowerType power
Definition: polynomial.h:62
A scalar multi-variate polynomial, modeled after the msspoly in spotless.
Definition: polynomial.h:42
Eigen::Matrix< CoefficientType, Eigen::Dynamic, 1 > GetCoefficients() const
Eigen::Matrix< RootType, Eigen::Dynamic, 1 > RootsType
Definition: polynomial.h:51
RootsType Roots() const
Returns the roots of this (univariate) Polynomial.
VarType GetSimpleVariable() const
If the polynomial is "simple" – e.g.
Polynomial(void)
Construct the vacuous polynomial, "0".
Definition: polynomial.h:111
bool operator==(const Monomial &other) const
Definition: polynomial.h:83
friend std::ostream & operator<<(std::ostream &os, const Monomial &m)
Definition: polynomial.h:386
static std::string IdToVariableName(const VarType id)
Product< CoefficientType, T >::type EvaluateMultivariate(const std::map< VarType, T > &var_values) const
Evaluate a multivariate Polynomial at a specific point.
Definition: polynomial.h:218
Polynomial & operator/=(const CoefficientType &scalar)
friend const Polynomial operator-(const CoefficientType &scalar, const Polynomial &p)
Definition: polynomial.h:341
Polynomial EvaluatePartial(const std::map< VarType, CoefficientType > &var_values) const
Substitute values for some but not necessarily all variables of a Polynomial.
Definition: polynomial.h:54
int GetDegree() const
Returns the highest degree of any Monomial in this Polynomial.
static VarType VariableNameToId(const std::string name, const unsigned int m=1)
const Polynomial operator/(const CoefficientType &scalar) const
int value
Definition: copyable_unique_ptr_test.cc:61
Provides Drake&#39;s assertion implementation.
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
Polynomial & operator+=(const Polynomial &other)
Eigen::NumTraits< CoefficientType >::Real RealScalar
Definition: polynomial.h:49
CoefficientType coefficient
Definition: polynomial.h:80
unsigned int VarType
Definition: polynomial.h:45
Product< CoefficientType, T >::type EvaluateUnivariate(const T &x) const
Evaluate a univariate Polynomial at a specific point.
Definition: polynomial.h:185
VarType var
Definition: polynomial.h:61
_CoefficientType CoefficientType
Definition: polynomial.h:44
friend const Polynomial operator-(const Polynomial &p, const CoefficientType &scalar)
Definition: polynomial.h:334
const Polynomial operator*(const Polynomial &other) const
Polynomial(Eigen::MatrixBase< Derived > const &coefficients)
A legacy constructor for univariate polynomials: Takes a vector of coefficients for the constant...
Definition: polynomial.h:136
std::complex< RealScalar > RootType
Definition: polynomial.h:50
bool IsApprox(const Polynomial &other, const RealScalar &tol) const
Checks if a (univariate) Polynomial is approximately equal to this one.
friend Polynomial< CoefficientType > pow(const Polynomial< CoefficientType > &p, typename Polynomial< CoefficientType >::PowerType n)
friend const Polynomial operator+(const CoefficientType &scalar, const Polynomial &p)
Definition: polynomial.h:327
friend const Polynomial operator*(const Polynomial &p, const CoefficientType &scalar)
Definition: polynomial.h:348
bool IsAffine() const
Returns true iff this is a sum of terms of degree 1, plus a constant.
Polynomial Integral(const CoefficientType &integration_constant=0.0) const
Takes the integral of this (univariate, non-constant) Polynomial.
std::vector< Term > terms
Definition: polynomial.h:81
friend const Polynomial operator*(const CoefficientType &scalar, const Polynomial &p)
Definition: polynomial.h:354
bool operator==(const Polynomial &other) const
decltype(static_cast< Rhs >(0)*static_cast< Lhs >(0)) typedef type
Definition: polynomial.h:55
Polynomial & operator*=(const Polynomial &other)
Polynomial< CoefficientType > pow(const Polynomial< CoefficientType > &base, typename Polynomial< CoefficientType >::PowerType exponent)
Provides power function for Polynomial.
Definition: polynomial.h:449
friend const Polynomial operator+(const Polynomial &p, const CoefficientType &scalar)
Definition: polynomial.h:320
bool operator<(const Monomial &other) const
A comparison to allow std::lexicographical_compare on this class; does not reflect any sort of mathem...
Definition: polynomial.h:89
An individual variable raised to an integer power; e.g. x**2.
Definition: polynomial.h:59
const Polynomial operator-() const
An additive atom of a Polynomial: The product of any number of Terms and a coefficient.
Definition: polynomial.h:78