diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index f168ef36ba64..bfaa5e6d46e8 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -207,6 +207,7 @@ drake_cc_library( ":evaluator_base", ], deps = [ + ":constraint", "//common/symbolic:polynomial", "//math:gradient", ], diff --git a/solvers/constraint.h b/solvers/constraint.h index 4ec3a04cf035..49877e2fddf2 100644 --- a/solvers/constraint.h +++ b/solvers/constraint.h @@ -7,7 +7,6 @@ #include #include #include -#include #include #include diff --git a/solvers/cost.cc b/solvers/cost.cc index a56b05af94d2..ec5010c235b2 100644 --- a/solvers/cost.cc +++ b/solvers/cost.cc @@ -5,6 +5,7 @@ #include "drake/common/symbolic/decompose.h" #include "drake/math/autodiff_gradient.h" #include "drake/math/differentiable_norm.h" +#include "drake/solvers/constraint.h" using Eigen::MatrixXd; using Eigen::VectorXd; @@ -307,78 +308,38 @@ std::ostream& PerspectiveQuadraticCost::DoDisplay( return DisplayCost(*this, os, "PerspectiveQuadraticCost", vars); } -ExpressionCost::ExpressionCost(const symbolic::Expression &e) - : Cost(e.GetVariables().size()), expression_(e) { - // Setup map_var_to_index_ and vars_ so that - // map_var_to_index_[vars_(i).get_id()] = i. - std::tie(vars_, map_var_to_index_) = - symbolic::ExtractVariablesFromExpression(expression_); +ExpressionCost::ExpressionCost(const symbolic::Expression& e) + : Cost(e.GetVariables().size()), + /* We reuse the Constraint evaluator's implementation. */ + evaluator_(std::make_unique( + Vector1{e}, + /* The ub, lb are unused but still required. */ + Vector1d(0.0), Vector1d(0.0))) {} - // Compute gradients and set up the environment. - derivatives_.resize(vars_.size()); - for (int i = 0; i < vars_.size(); i++) { - derivatives_[i] = expression_.Differentiate(vars_[i]); - environment_.insert(vars_[i], 0.0); - } +const VectorXDecisionVariable& ExpressionCost::vars() const { + return dynamic_cast(*evaluator_).vars(); } -void ExpressionCost::DoEval(const Eigen::Ref& x, - Eigen::VectorXd* y) const { - DRAKE_DEMAND(x.rows() == vars_.rows()); - - // Set environment with current x values. - for (int i = 0; i < vars_.size(); i++) { - environment_[vars_[i]] = x(map_var_to_index_.at(vars_[i].get_id())); - } +const symbolic::Expression& ExpressionCost::expression() const { + return dynamic_cast(*evaluator_) + .expressions() + .coeffRef(0); +} - // Evaluate into the output, y. - y->resize(1); - (*y)[0] = expression_.Evaluate(environment_); +void ExpressionCost::DoEval(const Eigen::Ref& x, + Eigen::VectorXd* y) const { + evaluator_->Eval(x, y); } void ExpressionCost::DoEval(const Eigen::Ref& x, - AutoDiffVecXd* y) const { - DRAKE_DEMAND(x.rows() == vars_.rows()); - - // Set environment with current x values. - for (int i = 0; i < vars_.size(); i++) { - environment_[vars_[i]] = x(map_var_to_index_.at(vars_[i].get_id())).value(); - } - - // Evaluate value and derivatives into the output, y. - // Using ∂yᵢ/∂zⱼ = ∑ₖ ∂fᵢ/∂xₖ ∂xₖ/∂zⱼ. - y->resize(1); - Eigen::VectorXd dydx(x.size()); - (*y)[0].value() = expression_.Evaluate(environment_); - for (int k = 0; k < x.size(); k++) { - dydx[k] = derivatives_[k].Evaluate(environment_); - } - - (*y)[0].derivatives().resize(x(0).derivatives().size()); - for (int j = 0; j < x(0).derivatives().size(); j++) { - (*y)[0].derivatives()[j] = 0.0; - for (int k = 0; k < x.size(); k++) { - (*y)[0].derivatives()[j] += dydx[k] * x(k).derivatives()[j]; - } - } + AutoDiffVecXd* y) const { + evaluator_->Eval(x, y); } void ExpressionCost::DoEval( const Eigen::Ref>& x, VectorX* y) const { - DRAKE_DEMAND(x.rows() == vars_.rows()); - symbolic::Substitution subst; - for (int i = 0; i < vars_.size(); ++i) { - if (!vars_[i].equal_to(x[i])) { - subst.emplace(vars_[i], x[i]); - } - } - y->resize(1); - if (subst.empty()) { - (*y)[0] = expression_; - } else { - (*y)[0] = expression_.Substitute(subst); - } + evaluator_->Eval(x, y); } std::ostream& ExpressionCost::DoDisplay( @@ -386,6 +347,5 @@ std::ostream& ExpressionCost::DoDisplay( return DisplayCost(*this, os, "ExpressionCost", vars); } - } // namespace solvers } // namespace drake diff --git a/solvers/cost.h b/solvers/cost.h index b1110a34546f..bd18844cf2fb 100644 --- a/solvers/cost.h +++ b/solvers/cost.h @@ -564,12 +564,10 @@ class ExpressionCost : public Cost { * Any Binding that connects this constraint to decision variables should * pass this list of variables to the Binding. */ - const VectorXDecisionVariable& vars() const { return vars_; } + const VectorXDecisionVariable& vars() const; /** @return the symbolic expression. */ - const symbolic::Expression& expression() const { - return expression_; - } + const symbolic::Expression& expression() const; protected: void DoEval(const Eigen::Ref& x, @@ -585,15 +583,7 @@ class ExpressionCost : public Cost { const VectorX&) const override; private: - symbolic::Expression expression_{}; - RowVectorX derivatives_{0}; - - // map_var_to_index_[vars_(i).get_id()] = i. - VectorXDecisionVariable vars_{0}; - std::unordered_map map_var_to_index_; - - // Only for caching, does not carrying hidden state. - mutable symbolic::Environment environment_; + std::unique_ptr evaluator_; }; /**