Implements ExpressionCost

Most importantly, now AddCost(Expression) will succeed, even if it is
passed in an Expression that is non-Polynomial.

Resolves RobotLocomotion#17897.

solvers/BUILD.bazel

@@ -203,9 +203,11 @@ drake_cc_library(
     srcs = ["cost.cc"],
     hdrs = ["cost.h"],
     interface_deps = [
+        ":decision_variable",
         ":evaluator_base",
     ],
     deps = [
+        "//common/symbolic:polynomial",
         "//math:gradient",
     ],
 )

solvers/cost.cc

@@ -2,6 +2,7 @@
 
 #include <memory>
 
+#include "drake/common/symbolic/decompose.h"
 #include "drake/math/autodiff_gradient.h"
 #include "drake/math/differentiable_norm.h"
 
@@ -306,5 +307,85 @@ 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_);
+
+  // 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);
+  }
+}
+
+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()));
+  }
+
+  // Evaluate into the output, y.
+  y->resize(1);
+  (*y)[0] = expression_.Evaluate(environment_);
+}
+
+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];
+    }
+  }
+}
+
+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);
+  }
+}
+
+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

@@ -5,11 +5,13 @@
 #include <memory>
 #include <optional>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
 #include <Eigen/SparseCore>
 
+#include "drake/solvers/decision_variable.h"
 #include "drake/solvers/evaluator_base.h"
 
 namespace drake {
@@ -542,6 +544,58 @@ class PolynomialCost : public EvaluatorCost<PolynomialEvaluator> {
   }
 };
 
+/**
+ * Impose a generic (potentially nonlinear) cost represented as a symbolic
+ * Expression.  Expression::Evaluate is called on every constraint evaluation.
+ *
+ * Uses symbolic::Jacobian to provide the gradients to the AutoDiff method.
+ *
+ * @ingroup solver_evaluators
+ */
+class ExpressionCost : public Cost {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExpressionCost)
+
+  explicit ExpressionCost(const symbolic::Expression& e);
+
+  /**
+   * @return the list of the variables involved in the vector of expressions,
+   * in the order that they are expected to be received during DoEval.
+   * Any Binding that connects this constraint to decision variables should
+   * pass this list of variables to the Binding.
+   */
+  const VectorXDecisionVariable& vars() const { return vars_; }
+
+  /** @return the symbolic expression. */
+  const symbolic::Expression& expression() const {
+    return expression_;
+  }
+
+ protected:
+  void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
+              Eigen::VectorXd* y) const override;
+
+  void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
+              AutoDiffVecXd* y) const override;
+
+  void DoEval(const Eigen::Ref<const VectorX<symbolic::Variable>>& x,
+              VectorX<symbolic::Expression>* y) const override;
+
+  std::ostream& DoDisplay(std::ostream&,
+                          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_;
+};
+
 /**
  * Converts an input of type @p F to a nonlinear cost.
  * @tparam FF The forwarded function type (e.g., `const F&, `F&&`, ...).

solvers/create_cost.cc

@@ -97,10 +97,8 @@ Binding<PolynomialCost> ParsePolynomialCost(const symbolic::Expression& e) {
 
 Binding<Cost> ParseCost(const symbolic::Expression& e) {
   if (!e.is_polynomial()) {
-    ostringstream oss;
-    oss << "Expression " << e << " is not a polynomial. ParseCost does not"
-        << " support non-polynomial expression.\n";
-    throw runtime_error(oss.str());
+    auto cost = make_shared<ExpressionCost>(e);
+    return CreateBinding(cost, cost->vars());
   }
   const symbolic::Polynomial poly{e};
   const int total_degree{poly.TotalDegree()};

solvers/test/cost_test.cc

@@ -733,6 +733,45 @@ GTEST_TEST(EvaluatorCost, Eval) {
   EXPECT_NEAR(y2(0), a.dot(evaluator2_y) + b, 1E-12);
 }
 
+GTEST_TEST(ExpressionCost, Basic) {
+  using std::sin;
+  using std::cos;
+  Variable x("x"), y("y");
+  symbolic::Expression e = x * sin(y);
+  ExpressionCost cost(e);
+
+  EXPECT_TRUE(e.EqualTo(cost.expression()));
+  EXPECT_EQ(cost.vars().size(), 2);
+  EXPECT_EQ(cost.vars()[0], x);
+  EXPECT_EQ(cost.vars()[1], y);
+
+  EXPECT_EQ(cost.num_vars(), 2);
+  Vector2d x_d(1.2, 3.5);
+  VectorXd y_d;
+  Vector1d y_expected(1.2 * sin(3.5));
+  cost.Eval(x_d, &y_d);
+  EXPECT_TRUE(CompareMatrices(y_d, y_expected));
+
+  AutoDiffVecXd x_ad = math::InitializeAutoDiff(x_d);
+  AutoDiffVecXd y_ad;
+  RowVector2d y_deriv_expected(sin(3.5), 1.2*cos(3.5));
+  cost.Eval(x_ad, &y_ad);
+  EXPECT_TRUE(CompareMatrices(math::ExtractValue(y_ad), y_expected));
+  EXPECT_TRUE(CompareMatrices(math::ExtractGradient(y_ad), y_deriv_expected));
+
+  Variable x_test("x"), y_test("y");
+  Vector2<Variable> x_e(x_test, y_test);
+  VectorX<Expression> y_e;
+  Expression e_expected = x_test * sin(y_test);
+  cost.Eval(x_e, &y_e);
+  EXPECT_EQ(y_e.size(), 1);
+  EXPECT_TRUE(y_e[0].EqualTo(e_expected));
+
+  std::ostringstream os;
+  cost.Display(os, x_e);
+  EXPECT_EQ(fmt::format("{}", os.str()), "ExpressionCost (x * sin(y))");
+}
+
 }  // namespace
 }  // namespace solvers
 }  // namespace drake

solvers/test/mathematical_program_test.cc

@@ -3135,9 +3135,6 @@ GTEST_TEST(TestMathematicalProgram, TestAddCostThrowError) {
   MathematicalProgram prog;
   auto x = prog.NewContinuousVariables<2>();
 
-  // Add a non-polynomial cost.
-  EXPECT_THROW(prog.AddCost(sin(x(0))), runtime_error);
-
   // Add a cost containing variable not included in the mathematical program.
   Variable y("y");
   DRAKE_EXPECT_THROWS_MESSAGE(prog.AddCost(x(0) + y),
@@ -3162,6 +3159,27 @@ GTEST_TEST(TestMathematicalProgram, TestAddGenericCost) {
   EXPECT_EQ(prog.quadratic_costs().size(), 1);
 }
 
+GTEST_TEST(TestMathematicalProgram, TestAddGenericCostExpression) {
+  MathematicalProgram prog;
+  Variable x = prog.NewContinuousVariables<1>()[0];
+  Variable y = prog.NewContinuousVariables<1>()[0];
+
+  using std::atan2;
+  auto b = prog.AddCost(atan2(y, x));
+  EXPECT_EQ(prog.generic_costs().size(), 1);
+  Vector2d x_test(0.5, 0.2);
+  Vector1d y_expected(atan2(0.2, 0.5));
+  EXPECT_TRUE(CompareMatrices(prog.EvalBinding(b, x_test), y_expected));
+}
+
+// Confirm that even constant costs are supported.
+GTEST_TEST(TestMathematicalProgram, TestAddCostConstant) {
+  MathematicalProgram prog;
+
+  auto b = prog.AddCost(Expression(0.5));
+  EXPECT_EQ(prog.linear_costs().size(), 1);
+}
+
 GTEST_TEST(TestMathematicalProgram, TestClone) {
   MathematicalProgram prog;
   auto x = prog.NewContinuousVariables<3>("x");