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 
17 
42 template <typename _CoefficientType = double>
43 class Polynomial {
44  public:
45  typedef _CoefficientType CoefficientType;
46  typedef unsigned int VarType;
49  typedef int PowerType;
50  typedef typename Eigen::NumTraits<CoefficientType>::Real RealScalar;
51  typedef std::complex<RealScalar> RootType;
52  typedef Eigen::Matrix<RootType, Eigen::Dynamic, 1> RootsType;
53 
54  template <typename Rhs, typename Lhs>
55  struct Product {
56  typedef decltype((Rhs)0 * (Lhs)0) type;
57  };
58 
60  class Term {
61  public:
62  VarType var;
63  PowerType power;
64 
65  bool operator==(const Term& other) const {
66  return (var == other.var) && (power == other.power);
67  }
68 
71  bool operator<(const Term& other) const {
72  return ((var < other.var) ||
73  ((var == other.var) && (power < other.power)));
74  }
75  };
76 
79  class Monomial {
80  public:
81  CoefficientType coefficient;
82  std::vector<Term> terms; // a list of N variable ids
83 
84  bool operator==(const Monomial& other) const {
85  return (coefficient == other.coefficient) && (terms == other.terms);
86  }
87 
90  bool operator<(const Monomial& other) const {
91  return ((coefficient < other.coefficient) ||
92  ((coefficient == other.coefficient) && (terms < other.terms)));
93  }
94 
95  int GetDegree() const;
96  int GetDegreeOf(VarType var) const;
97  bool HasSameExponents(const Monomial& other) const;
98 
100  Monomial Factor(const Monomial& divisor) const;
101  };
102 
103  private:
105  std::vector<Monomial> monomials_;
106 
108  bool is_univariate_;
109 
110  public:
112  Polynomial(void) : is_univariate_(true) {}
113 
115  // This is required for some Eigen operations when used in a
116  // polynomial matrix.
117  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
118  Polynomial(const CoefficientType& scalar);
119 
121  Polynomial(const CoefficientType coeff, const std::vector<Term>& terms);
122 
124  Polynomial(typename std::vector<Monomial>::const_iterator start,
125  typename std::vector<Monomial>::const_iterator finish);
126 
129  explicit Polynomial(const std::string varname, const unsigned int num = 1);
130 
132  Polynomial(const CoefficientType& coeff, const VarType& v);
133 
136  template <typename Derived>
137  explicit Polynomial(Eigen::MatrixBase<Derived> const& coefficients) {
138  VarType v = VariableNameToId("t");
139  for (int i = 0; i < coefficients.size(); i++) {
140  Monomial m;
141  m.coefficient = coefficients(i);
142  if (i > 0) {
143  Term t;
144  t.var = v;
145  t.power = i;
146  m.terms.push_back(t);
147  }
148  monomials_.push_back(m);
149  }
150  is_univariate_ = true;
151  }
152 
155  int GetNumberOfCoefficients() const;
156 
161  int GetDegree() const;
162 
164  bool IsAffine() const;
165 
168  VarType GetSimpleVariable() const;
169 
170  const std::vector<Monomial>& GetMonomials() const;
171 
172  Eigen::Matrix<CoefficientType, Eigen::Dynamic, 1> GetCoefficients() const;
173 
175  std::set<VarType> GetVariables() const;
176 
185  template <typename T>
187  const T& x) const {
188  typedef typename Product<CoefficientType, T>::type ProductType;
189 
190  if (!is_univariate_)
191  throw std::runtime_error(
192  "this method can only be used for univariate polynomials");
193  ProductType value = 0;
194  using std::pow;
195  for (typename std::vector<Monomial>::const_iterator iter =
196  monomials_.begin();
197  iter != monomials_.end(); iter++) {
198  if (iter->terms.empty())
199  value += iter->coefficient;
200  else
201  value += iter->coefficient *
202  pow((ProductType) x,
203  (PowerType) iter->terms[0].power);
204  }
205  return value;
206  }
207 
218  template <typename T>
220  const std::map<VarType, T>& var_values) const {
221  typedef typename std::remove_const<
222  typename Product<CoefficientType, T>::type>::type ProductType;
223  ProductType value = 0;
224  for (const Monomial& monomial : monomials_) {
225  ProductType monomial_value = monomial.coefficient;
226  for (const Term& term : monomial.terms) {
227  monomial_value *= std::pow(
228  static_cast<ProductType>(var_values.at(term.var)),
229  term.power);
230  }
231  value += monomial_value;
232  }
233  return value;
234  }
235 
248  const std::map<VarType, drake::AutoDiffXd>& var_values) const {
250  for (const Monomial& monomial : monomials_) {
251  drake::AutoDiffXd monomial_value(monomial.coefficient);
252  for (const Term& term : monomial.terms) {
253  monomial_value *= pow(var_values.at(term.var), term.power);
254  }
255  value += monomial_value;
256  }
257  return value;
258  }
259 
271  const std::map<VarType, CoefficientType>& var_values) const;
272 
274  void Subs(const VarType& orig, const VarType& replacement);
275 
284  Polynomial Derivative(int derivative_order = 1) const;
285 
295  Polynomial Integral(const CoefficientType& integration_constant = 0.0) const;
296 
297  bool operator==(const Polynomial& other) const;
298 
299  Polynomial& operator+=(const Polynomial& other);
300 
301  Polynomial& operator-=(const Polynomial& other);
302 
303  Polynomial& operator*=(const Polynomial& other);
304 
305  Polynomial& operator+=(const CoefficientType& scalar);
306 
307  Polynomial& operator-=(const CoefficientType& scalar);
308 
309  Polynomial& operator*=(const CoefficientType& scalar);
310 
311  Polynomial& operator/=(const CoefficientType& scalar);
312 
313  const Polynomial operator+(const Polynomial& other) const;
314 
315  const Polynomial operator-(const Polynomial& other) const;
316 
317  const Polynomial operator-() const;
318 
319  const Polynomial operator*(const Polynomial& other) const;
320 
321  friend const Polynomial operator+(const Polynomial& p,
322  const CoefficientType& scalar) {
323  Polynomial ret = p;
324  ret += scalar;
325  return ret;
326  }
327 
328  friend const Polynomial operator+(const CoefficientType& scalar,
329  const Polynomial& p) {
330  Polynomial ret = p;
331  ret += scalar;
332  return ret;
333  }
334 
335  friend const Polynomial operator-(const Polynomial& p,
336  const CoefficientType& scalar) {
337  Polynomial ret = p;
338  ret -= scalar;
339  return ret;
340  }
341 
342  friend const Polynomial operator-(const CoefficientType& scalar,
343  const Polynomial& p) {
344  Polynomial ret = -p;
345  ret += scalar;
346  return ret;
347  }
348 
349  friend const Polynomial operator*(const Polynomial& p,
350  const CoefficientType& scalar) {
351  Polynomial ret = p;
352  ret *= scalar;
353  return ret;
354  }
355  friend const Polynomial operator*(const CoefficientType& scalar,
356  const Polynomial& p) {
357  Polynomial ret = p;
358  ret *= scalar;
359  return ret;
360  }
361 
362  const Polynomial operator/(const CoefficientType& scalar) const;
363 
366  bool operator<(const Polynomial& other) const {
367  // Just delegate to the default vector std::lexicographical_compare.
368  return monomials_ < other.monomials_;
369  }
370 
377  RootsType Roots() const;
378 
385  bool IsApprox(const Polynomial& other, const RealScalar& tol) const;
386 
387  friend std::ostream& operator<<(std::ostream& os, const Monomial& m) {
388  // if (m.coefficient == 0) return os;
389 
390  bool print_star = false;
391  if (m.coefficient == -1) {
392  os << "-";
393  } else if (m.coefficient != 1 || m.terms.empty()) {
394  os << '(' << m.coefficient << ")";
395  print_star = true;
396  }
397 
398  for (typename std::vector<Term>::const_iterator iter = m.terms.begin();
399  iter != m.terms.end(); iter++) {
400  if (print_star)
401  os << '*';
402  else
403  print_star = true;
404  os << IdToVariableName(iter->var);
405  if (iter->power != 1) {
406  os << "^" << iter->power;
407  }
408  }
409  return os;
410  }
411 
412  friend std::ostream& operator<<(std::ostream& os, const Polynomial& poly) {
413  if (poly.monomials_.empty()) {
414  os << "0";
415  return os;
416  }
417 
418  for (typename std::vector<Monomial>::const_iterator iter =
419  poly.monomials_.begin();
420  iter != poly.monomials_.end(); iter++) {
421  os << *iter;
422  if (iter + 1 != poly.monomials_.end() && (iter + 1)->coefficient != -1)
423  os << '+';
424  }
425  return os;
426  }
427 
429 
430  static bool IsValidVariableName(const std::string name);
431 
432  static VarType VariableNameToId(const std::string name,
433  const unsigned int m = 1);
434 
435  static std::string IdToVariableName(const VarType id);
437 
438  template <typename CoefficientType>
442 
443  private:
445  void MakeMonomialsUnique(void);
446 };
447 
449 template <typename CoefficientType>
451  const Polynomial<CoefficientType>& base,
452  typename Polynomial<CoefficientType>::PowerType exponent) {
453  DRAKE_DEMAND(exponent >= 0);
454  if (exponent == 0) {
455  return Polynomial<CoefficientType>{1.0};
456  }
457  const Polynomial<CoefficientType> pow_half{pow(base, exponent / 2)};
458  if (exponent % 2 == 1) {
459  return base * pow_half * pow_half; // Odd exponent case.
460  } else {
461  return pow_half * pow_half; // Even exponent case.
462  }
463 }
464 
465 template <typename CoefficientType, int Rows, int Cols>
466 std::ostream& operator<<(
467  std::ostream& os,
468  const Eigen::Matrix<Polynomial<CoefficientType>, Rows, Cols>& poly_mat) {
469  for (int i = 0; i < poly_mat.rows(); i++) {
470  os << "[ ";
471  for (int j = 0; j < poly_mat.cols(); j++) {
472  os << poly_mat(i, j);
473  if (j < (poly_mat.cols() - 1)) os << " , ";
474  }
475  os << " ]" << std::endl;
476  }
477  return os;
478 }
479 
481 
483 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:247
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:22
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:483
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:366
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:71
std::vector< Number > x
Definition: ipopt_solver.cc:150
bool operator==(const Term &other) const
Definition: polynomial.h:65
This file contains abbreviated definitions for certain uses of AutoDiffScalar that are commonly used ...
friend std::ostream & operator<<(std::ostream &os, const Polynomial &poly)
Definition: polynomial.h:412
Polynomial & operator-=(const Polynomial &other)
Polynomial< double > Polynomiald
Definition: polynomial.h:480
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:49
Polynomial Derivative(int derivative_order=1) const
Takes the derivative of this (univariate) Polynomial.
PowerType power
Definition: polynomial.h:63
A scalar multi-variate polynomial, modeled after the msspoly in spotless.
Definition: polynomial.h:43
Eigen::Matrix< CoefficientType, Eigen::Dynamic, 1 > GetCoefficients() const
Eigen::Matrix< RootType, Eigen::Dynamic, 1 > RootsType
Definition: polynomial.h:52
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:112
bool operator==(const Monomial &other) const
Definition: polynomial.h:84
friend std::ostream & operator<<(std::ostream &os, const Monomial &m)
Definition: polynomial.h:387
static std::string IdToVariableName(const VarType id)
Overloads for STL mathematical operations on AutoDiffScalar.
Product< CoefficientType, T >::type EvaluateMultivariate(const std::map< VarType, T > &var_values) const
Evaluate a multivariate Polynomial at a specific point.
Definition: polynomial.h:219
Polynomial & operator/=(const CoefficientType &scalar)
friend const Polynomial operator-(const CoefficientType &scalar, const Polynomial &p)
Definition: polynomial.h:342
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:55
int GetDegree() const
Returns the highest degree of any Monomial in this Polynomial.
int v
Definition: rgbd_camera_test.cc:165
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:47
Polynomial & operator+=(const Polynomial &other)
Eigen::NumTraits< CoefficientType >::Real RealScalar
Definition: polynomial.h:50
CoefficientType coefficient
Definition: polynomial.h:81
unsigned int VarType
Definition: polynomial.h:46
Product< CoefficientType, T >::type EvaluateUnivariate(const T &x) const
Evaluate a univariate Polynomial at a specific point.
Definition: polynomial.h:186
VarType var
Definition: polynomial.h:62
_CoefficientType CoefficientType
Definition: polynomial.h:45
friend const Polynomial operator-(const Polynomial &p, const CoefficientType &scalar)
Definition: polynomial.h:335
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:137
std::complex< RealScalar > RootType
Definition: polynomial.h:51
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:328
friend const Polynomial operator*(const Polynomial &p, const CoefficientType &scalar)
Definition: polynomial.h:349
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:82
friend const Polynomial operator*(const CoefficientType &scalar, const Polynomial &p)
Definition: polynomial.h:355
bool operator==(const Polynomial &other) const
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:450
friend const Polynomial operator+(const Polynomial &p, const CoefficientType &scalar)
Definition: polynomial.h:321
decltype((Rhs) 0 *(Lhs) 0) typedef type
Definition: polynomial.h:56
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:90
An individual variable raised to an integer power; e.g. x**2.
Definition: polynomial.h:60
const Polynomial operator-() const
An additive atom of a Polynomial: The product of any number of Terms and a coefficient.
Definition: polynomial.h:79