fixup!

solvers/BUILD.bazel

@@ -207,6 +207,7 @@ drake_cc_library(
         ":evaluator_base",
     ],
     deps = [
+        ":constraint",
         "//common/symbolic:polynomial",
         "//math:gradient",
     ],

solvers/constraint.h

@@ -7,7 +7,6 @@
 #include <optional>
 #include <stdexcept>
 #include <string>
-#include <unordered_map>
 #include <utility>
 #include <vector>
 

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,85 +308,44 @@ 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<ExpressionConstraint>(
+          Vector1<symbolic::Expression>{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<const ExpressionConstraint&>(*evaluator_).vars();
 }
 
-void ExpressionCost::DoEval(const Eigen::Ref<const Eigen::VectorXd>& 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<const ExpressionConstraint&>(*evaluator_)
+      .expressions()
+      .coeffRef(0);
+}
 
-  // Evaluate into the output, y.
-  y->resize(1);
-  (*y)[0] = expression_.Evaluate(environment_);
+void ExpressionCost::DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
+                            Eigen::VectorXd* y) const {
+  evaluator_->Eval(x, y);
 }
 
 void ExpressionCost::DoEval(const Eigen::Ref<const AutoDiffVecXd>& 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<const VectorX<symbolic::Variable>>& x,
     VectorX<symbolic::Expression>* 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(
     std::ostream& os, const VectorX<symbolic::Variable>& vars) const {
   return DisplayCost(*this, os, "ExpressionCost", vars);
 }
 
-
 }  // namespace solvers
 }  // namespace drake

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<const Eigen::VectorXd>& x,
@@ -585,15 +583,7 @@ class ExpressionCost : public Cost {
                           const VectorX<symbolic::Variable>&) const override;
 
  private:
-  symbolic::Expression expression_{};
-  RowVectorX<symbolic::Expression> derivatives_{0};
-
-  // map_var_to_index_[vars_(i).get_id()] = i.
-  VectorXDecisionVariable vars_{0};
-  std::unordered_map<symbolic::Variable::Id, int> map_var_to_index_;
-
-  // Only for caching, does not carrying hidden state.
-  mutable symbolic::Environment environment_;
+  std::unique_ptr<EvaluatorBase> evaluator_;
 };
 
 /**