Skip to content

Commit

Permalink
Polynomial degree (RobotLocomotion#5073)
Browse files Browse the repository at this point in the history 
* WIP: work on symbolic polynomial

* WIP: use symbolic::Expression to represent polynomial

* Try to add a degree function, this approach does not work

* add Degree(const Variables& vars) to symbolic::Expression

* remove the separate symbolic_polynomial files

* Add Degree() to symbolic::Expression

* cpplint

* Address Soonho's comments

* Address Soonho's comments

* Address David's comment
  • Loading branch information
@hongkai-dai
hongkai-dai authored Feb 7, 2017
1 parent a925368 commit 89a02be
Show file tree
Hide file tree
Showing 5 changed files with 278 additions and 1 deletion.
12 changes: 12 additions & 0 deletions drake/common/symbolic_expression.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,18 @@ Polynomial<double> Expression::ToPolynomial() const {
return ptr_->ToPolynomial();
}

int Expression::Degree(const Variables& vars) const {
DRAKE_ASSERT(ptr_ != nullptr);
DRAKE_DEMAND(is_polynomial());
return ptr_->Degree(vars);
}

int Expression::Degree() const {
DRAKE_ASSERT(ptr_ != nullptr);
DRAKE_DEMAND(is_polynomial());
return ptr_->Degree(GetVariables());
}

double Expression::Evaluate(const Environment& env) const {
DRAKE_ASSERT(ptr_ != nullptr);
return ptr_->Evaluate(env);
Expand Down
22 changes: 22 additions & 0 deletions drake/common/symbolic_expression.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,6 +176,28 @@ class Expression {
*/
Polynomial<double> ToPolynomial() const;

/**
* Returns the total degrees of the polynomial w.r.t the variables in
* @p vars. For example, the total degree of
* e = x^2*y + 2 * x*y*z^3 + x * z^2
* w.r.t (x, y) is 3 (from x^2 * y)
* w.r.t (x, z) is 4 (from x*y*z^3)
* w.r.t (z) is 3 (from x*y*z^3)
* Throws a runtime error if is_polynomial() is false.
* @param vars A set of variables.
* @return The total degree.
*/
int Degree(const Variables& vars) const;

/**
* Returns the total degress of all the variables in the polynomial.
* For example, the total degree of
* x^2*y + 2*x*y*z^3 + x*z^2
* is 5, from x*y*z^3
* Throws a runtime error is is_polynomial() is false.
* @return The total degree.
*/
int Degree() const;
/** Evaluates under a given environment (by default, an empty environment).
* @throws std::runtime_error if NaN is detected during evaluation.
*/
Expand Down
113 changes: 113 additions & 0 deletions drake/common/symbolic_expression_cell.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,6 +211,10 @@ Polynomial<double> ExpressionVar::ToPolynomial() const {
return Polynomial<double>(1.0, var_.get_id());
}

int ExpressionVar::Degree(const Variables &vars) const {
return vars.include(var_) ? 1 : 0;
}

double ExpressionVar::Evaluate(const Environment& env) const {
Environment::const_iterator const it{env.find(var_)};
if (it != env.cend()) {
Expand Down Expand Up @@ -260,6 +264,10 @@ Polynomial<double> ExpressionConstant::ToPolynomial() const {
return Polynomial<double>(v_);
}

int ExpressionConstant::Degree(const Variables& vars) const {
return 0;
}

double ExpressionConstant::Evaluate(const Environment& env) const {
DRAKE_DEMAND(!std::isnan(v_));
return v_;
Expand Down Expand Up @@ -301,6 +309,10 @@ Polynomial<double> ExpressionNaN::ToPolynomial() const {
throw runtime_error("NaN is detected while converting to Polynomial.");
}

int ExpressionNaN::Degree(const Variables& vars) const {
throw runtime_error("NaN is detected while enquiring polynomial degree.");
}

double ExpressionNaN::Evaluate(const Environment& env) const {
throw runtime_error("NaN is detected during Symbolic computation.");
}
Expand Down Expand Up @@ -387,6 +399,14 @@ Polynomial<double> ExpressionAdd::ToPolynomial() const {
});
}

int ExpressionAdd::Degree(const Variables& vars) const {
int degree = 0;
for (const auto& p : expr_to_coeff_map_) {
degree = std::max(degree, p.first.Degree(vars));
}
return degree;
}

double ExpressionAdd::Evaluate(const Environment& env) const {
return accumulate(
expr_to_coeff_map_.begin(), expr_to_coeff_map_.end(), constant_,
Expand Down Expand Up @@ -622,6 +642,18 @@ Polynomial<double> ExpressionMul::ToPolynomial() const {
});
}

int ExpressionMul::Degree(const Variables &vars) const {
// The precondition is is_polynomial() == True.
return accumulate(
base_to_exponent_map_.begin(), base_to_exponent_map_.end(), 0,
[vars](const int& degree, const pair<Expression, Expression>& p) {
const Expression& base{p.first};
const Expression& expnt{p.second};
return degree +
base.Degree(vars) * static_cast<int>(get_constant_value(expnt));
});
}

double ExpressionMul::Evaluate(const Environment& env) const {
return accumulate(
base_to_exponent_map_.begin(), base_to_exponent_map_.end(), constant_,
Expand Down Expand Up @@ -780,6 +812,11 @@ Polynomial<double> ExpressionDiv::ToPolynomial() const {
get_constant_value(get_second_argument());
}

int ExpressionDiv::Degree(const Variables& vars) const {
// The precondition is is_polynomial() == True.
return get_first_argument().Degree(vars) - get_second_argument().Degree(vars);
}

Expression ExpressionDiv::Substitute(const Substitution& s) const {
return get_first_argument().Substitute(s) /
get_second_argument().Substitute(s);
Expand Down Expand Up @@ -816,6 +853,10 @@ Polynomial<double> ExpressionLog::ToPolynomial() const {
throw runtime_error("Log expression is not polynomial-convertible.");
}

int ExpressionLog::Degree(const Variables &vars) const {
throw runtime_error("Log expression does not have a polynomial degree.");
}

Expression ExpressionLog::Substitute(const Substitution& s) const {
return log(get_argument().Substitute(s));
}
Expand All @@ -836,6 +877,10 @@ Polynomial<double> ExpressionAbs::ToPolynomial() const {
throw runtime_error("Abs expression is not polynomial-convertible.");
}

int ExpressionAbs::Degree(const Variables& vars) const {
throw runtime_error("Abs expression does not have a polynomial degree.");
}

Expression ExpressionAbs::Substitute(const Substitution& s) const {
return abs(get_argument().Substitute(s));
}
Expand All @@ -853,6 +898,10 @@ Polynomial<double> ExpressionExp::ToPolynomial() const {
throw runtime_error("Exp expression is not polynomial-convertible.");
}

int ExpressionExp::Degree(const Variables &vars) const {
throw runtime_error("Exp expression does not have a polynomial degree.");
}

Expression ExpressionExp::Substitute(const Substitution& s) const {
return exp(get_argument().Substitute(s));
}
Expand All @@ -879,6 +928,10 @@ Polynomial<double> ExpressionSqrt::ToPolynomial() const {
throw runtime_error("Sqrt expression is not polynomial-convertible.");
}

int ExpressionSqrt::Degree(const Variables& vars) const {
throw runtime_error("Sqrt expression does not have a polynomial degree.");
}

Expression ExpressionSqrt::Substitute(const Substitution& s) const {
return sqrt(get_argument().Substitute(s));
}
Expand Down Expand Up @@ -914,6 +967,13 @@ Polynomial<double> ExpressionPow::ToPolynomial() const {
return pow(get_first_argument().ToPolynomial(), exponent);
}

int ExpressionPow::Degree(const Variables &vars) const {
// As a precondition, this expression `is_polynomial`, so the exponent is an
// integer and this cast does not change its value.
const int expnt{static_cast<int>(get_constant_value(get_second_argument()))};
return get_first_argument().Degree(vars) * expnt;
}

Expression ExpressionPow::Substitute(const Substitution& s) const {
return pow(get_first_argument().Substitute(s),
get_second_argument().Substitute(s));
Expand All @@ -936,6 +996,10 @@ Polynomial<double> ExpressionSin::ToPolynomial() const {
throw runtime_error("Sin expression is not polynomial-convertible.");
}

int ExpressionSin::Degree(const Variables& vars) const {
throw runtime_error("Sin expression does not have a polynomial degree.");
}

Expression ExpressionSin::Substitute(const Substitution& s) const {
return sin(get_argument().Substitute(s));
}
Expand All @@ -953,6 +1017,10 @@ Polynomial<double> ExpressionCos::ToPolynomial() const {
throw runtime_error("Cos expression is not polynomial-convertible.");
}

int ExpressionCos::Degree(const Variables &vars) const {
throw runtime_error("Cos expression does not have a polynomial degree.");
}

Expression ExpressionCos::Substitute(const Substitution& s) const {
return cos(get_argument().Substitute(s));
}
Expand All @@ -970,6 +1038,10 @@ Polynomial<double> ExpressionTan::ToPolynomial() const {
throw runtime_error("Tan expression is not polynomial-convertible.");
}

int ExpressionTan::Degree(const Variables& vars) const {
throw runtime_error("Tan expression does not have a polynomial degree.");
}

Expression ExpressionTan::Substitute(const Substitution& s) const {
return tan(get_argument().Substitute(s));
}
Expand All @@ -996,6 +1068,10 @@ Polynomial<double> ExpressionAsin::ToPolynomial() const {
throw runtime_error("Asin expression is not polynomial-convertible.");
}

int ExpressionAsin::Degree(const Variables &vars) const {
throw runtime_error("Asin expression does not have a polynomial degree.");
}

Expression ExpressionAsin::Substitute(const Substitution& s) const {
return asin(get_argument().Substitute(s));
}
Expand Down Expand Up @@ -1025,6 +1101,10 @@ Polynomial<double> ExpressionAcos::ToPolynomial() const {
throw runtime_error("Acos expression is not polynomial-convertible.");
}

int ExpressionAcos::Degree(const Variables &vars) const {
throw runtime_error("Acos expression does not have a polynomial degree.");
}

Expression ExpressionAcos::Substitute(const Substitution& s) const {
return acos(get_argument().Substitute(s));
}
Expand All @@ -1045,6 +1125,10 @@ Polynomial<double> ExpressionAtan::ToPolynomial() const {
throw runtime_error("Atan expression is not polynomial-convertible.");
}

int ExpressionAtan::Degree(const Variables &vars) const {
throw runtime_error("Atan expression does not have a polynomial degree.");
}

Expression ExpressionAtan::Substitute(const Substitution& s) const {
return atan(get_argument().Substitute(s));
}
Expand All @@ -1062,6 +1146,10 @@ Polynomial<double> ExpressionAtan2::ToPolynomial() const {
throw runtime_error("Atan2 expression is not polynomial-convertible.");
}

int ExpressionAtan2::Degree(const Variables &vars) const {
throw runtime_error("Atan2 expression does not have a polynomial degree.");
}

Expression ExpressionAtan2::Substitute(const Substitution& s) const {
return atan2(get_first_argument().Substitute(s),
get_second_argument().Substitute(s));
Expand All @@ -1083,6 +1171,10 @@ Polynomial<double> ExpressionSinh::ToPolynomial() const {
throw runtime_error("Sinh expression is not polynomial-convertible.");
}

int ExpressionSinh::Degree(const Variables &vars) const {
throw runtime_error("Sinh expression does not have a polynomial degree.");
}

Expression ExpressionSinh::Substitute(const Substitution& s) const {
return sinh(get_argument().Substitute(s));
}
Expand All @@ -1100,6 +1192,10 @@ Polynomial<double> ExpressionCosh::ToPolynomial() const {
throw runtime_error("Cosh expression is not polynomial-convertible.");
}

int ExpressionCosh::Degree(const Variables &vars) const {
throw runtime_error("Cosh expression does not have a polynomial degree.");
}

Expression ExpressionCosh::Substitute(const Substitution& s) const {
return cosh(get_argument().Substitute(s));
}
Expand All @@ -1117,6 +1213,10 @@ Polynomial<double> ExpressionTanh::ToPolynomial() const {
throw runtime_error("Tanh expression is not polynomial-convertible.");
}

int ExpressionTanh::Degree(const Variables& vars) const {
throw runtime_error("Tanh expression does not have a polynomial degree.");
}

Expression ExpressionTanh::Substitute(const Substitution& s) const {
return tanh(get_argument().Substitute(s));
}
Expand All @@ -1134,6 +1234,10 @@ Polynomial<double> ExpressionMin::ToPolynomial() const {
throw runtime_error("Min expression is not polynomial-convertible.");
}

int ExpressionMin::Degree(const Variables &vars) const {
throw runtime_error("Min expression does not have a polynomial degree.");
}

Expression ExpressionMin::Substitute(const Substitution& s) const {
return min(get_first_argument().Substitute(s),
get_second_argument().Substitute(s));
Expand All @@ -1155,6 +1259,10 @@ Polynomial<double> ExpressionMax::ToPolynomial() const {
throw runtime_error("Max expression is not polynomial-convertible.");
}

int ExpressionMax::Degree(const Variables &vars) const {
throw runtime_error("Max expression does not have a polynomial degree.");
}

Expression ExpressionMax::Substitute(const Substitution& s) const {
return max(get_first_argument().Substitute(s),
get_second_argument().Substitute(s));
Expand Down Expand Up @@ -1222,6 +1330,11 @@ Polynomial<double> ExpressionIfThenElse::ToPolynomial() const {
throw runtime_error("IfThenElse expression is not polynomial-convertible.");
}

int ExpressionIfThenElse::Degree(const Variables &vars) const {
throw runtime_error(
"IfThenElse expression does not have a polynomial degree.");
}

double ExpressionIfThenElse::Evaluate(const Environment& env) const {
if (f_cond_.Evaluate(env)) {
return e_then_.Evaluate(env);
Expand Down
Loading

0 comments on commit 89a02be

Please sign in to comment.