Implements ExpressionCost (#19405)

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

Resolves #17897.

bindings/pydrake/solvers/solvers_py_mathematicalprogram.cc

@@ -822,9 +822,7 @@ void BindMathematicalProgram(py::module m) {
       .def("AddCost",
           static_cast<Binding<Cost> (MathematicalProgram::*)(
               const Expression&)>(&MathematicalProgram::AddCost),
-          // N.B. There is no corresponding C++ method, so the docstring here
-          // is a literal, not a reference to documentation_pybind.h
-          "Adds a cost expression.")
+          py::arg("e"), doc.MathematicalProgram.AddCost.doc_1args_e)
       .def(
           "AddCost",
           [](MathematicalProgram* self, const std::shared_ptr<Cost>& obj,

solvers/BUILD.bazel

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

solvers/cost.cc

@@ -4,6 +4,7 @@
 
 #include "drake/math/autodiff_gradient.h"
 #include "drake/math/differentiable_norm.h"
+#include "drake/solvers/constraint.h"
 
 using Eigen::MatrixXd;
 using Eigen::VectorXd;
@@ -306,5 +307,44 @@ std::ostream& PerspectiveQuadraticCost::DoDisplay(
   return DisplayCost(*this, os, "PerspectiveQuadraticCost", vars);
 }
 
+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))) {}
+
+const VectorXDecisionVariable& ExpressionCost::vars() const {
+  return dynamic_cast<const ExpressionConstraint&>(*evaluator_).vars();
+}
+
+const symbolic::Expression& ExpressionCost::expression() const {
+  return dynamic_cast<const ExpressionConstraint&>(*evaluator_)
+      .expressions()
+      .coeffRef(0);
+}
+
+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 {
+  evaluator_->Eval(x, y);
+}
+
+void ExpressionCost::DoEval(
+    const Eigen::Ref<const VectorX<symbolic::Variable>>& x,
+    VectorX<symbolic::Expression>* y) const {
+  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

@@ -10,6 +10,7 @@
 
 #include <Eigen/SparseCore>
 
+#include "drake/solvers/decision_variable.h"
 #include "drake/solvers/evaluator_base.h"
 
 namespace drake {
@@ -542,6 +543,48 @@ 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 the symbolic expression. */
+  const symbolic::Expression& expression() const;
+
+ 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:
+  std::unique_ptr<EvaluatorBase> evaluator_;
+};
+
 /**
  * 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/mathematical_program.h

@@ -1040,8 +1040,6 @@ class MathematicalProgram {
 
   /**
    * Add a quadratic cost term of the form 0.5*x'*Q*x + b'*x + c.
-   * Notice that in the optimization program, the constant term `c` in the cost
-   * is ignored.
    * @param e A quadratic symbolic expression.
    * @param is_convex Whether the cost is already known to be convex. If
    * is_convex=nullopt (the default), then Drake will determine if `e` is a
@@ -1195,14 +1193,7 @@ class MathematicalProgram {
 
   /**
    * Adds a cost in the symbolic form.
-   * Note that the constant part of the cost is ignored. So if you set
-   * `e = x + 2`, then only the cost on `x` is added, the constant term 2 is
-   * ignored.
-   * @param e The linear or quadratic expression of the cost.
-   * @pre `e` is linear or `e` is quadratic. Otherwise throws a runtime error.
    * @return The newly created cost, together with the bound variables.
-   *
-   * @exclude_from_pydrake_mkdoc{Not bound in pydrake.}
    */
   Binding<Cost> AddCost(const symbolic::Expression& e);
 

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(os.str(), "ExpressionCost (x * sin(y))");
+}
+
 }  // namespace
 }  // namespace solvers
 }  // namespace drake

solvers/test/mathematical_program_test.cc

@@ -2798,7 +2798,7 @@ void CheckAddedSymbolicQuadraticCostUserFun(const MathematicalProgram& prog,
   EXPECT_EQ(binding.variables(), prog.quadratic_costs().back().variables());
 
   auto cnstr = prog.quadratic_costs().back().evaluator();
-  // Check the added cost is 0.5 * x' * Q * x + b' * x
+  // Check the added cost is 0.5 * x' * Q * x + b' * x + c
   const auto& x_bound = binding.variables();
   const Expression e_added = 0.5 * x_bound.dot(cnstr->Q() * x_bound) +
                              cnstr->b().dot(x_bound) + cnstr->c();
@@ -2831,6 +2831,11 @@ GTEST_TEST(TestMathematicalProgram, AddSymbolicQuadraticCost) {
   Expression e2 = x.transpose() * x + 1;
   CheckAddedSymbolicQuadraticCost(&prog, e2, true);
   EXPECT_TRUE(prog.quadratic_costs().back().evaluator()->is_convex());
+  // Confirm that const terms are respected.
+  auto b = prog.AddQuadraticCost(e2);
+  VectorXd y(1);
+  b.evaluator()->Eval(Vector3d::Zero(), &y);
+  EXPECT_EQ(y[0], 1);
 
   // Identity diagonal term.
   Expression e3 = x(0) * x(0) + x(1) * x(1) + 2;
@@ -3135,9 +3140,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 +3164,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");