diff --git a/bindings/pydrake/solvers/solvers_py_mathematicalprogram.cc b/bindings/pydrake/solvers/solvers_py_mathematicalprogram.cc index 6051d11d3b94..74dda7820762 100644 --- a/bindings/pydrake/solvers/solvers_py_mathematicalprogram.cc +++ b/bindings/pydrake/solvers/solvers_py_mathematicalprogram.cc @@ -822,9 +822,7 @@ void BindMathematicalProgram(py::module m) { .def("AddCost", static_cast (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& obj, diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index eea735c7672e..8b6796cd2617 100644 --- a/solvers/BUILD.bazel +++ b/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", ], ) diff --git a/solvers/cost.cc b/solvers/cost.cc index 97b21b0fc3d3..3b59e04c16e3 100644 --- a/solvers/cost.cc +++ b/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( + Vector1{e}, + /* The ub, lb are unused but still required. */ + Vector1d(0.0), Vector1d(0.0))) {} + +const VectorXDecisionVariable& ExpressionCost::vars() const { + return dynamic_cast(*evaluator_).vars(); +} + +const symbolic::Expression& ExpressionCost::expression() const { + return dynamic_cast(*evaluator_) + .expressions() + .coeffRef(0); +} + +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 { + evaluator_->Eval(x, y); +} + +void ExpressionCost::DoEval( + const Eigen::Ref>& x, + VectorX* y) const { + evaluator_->Eval(x, y); +} + +std::ostream& ExpressionCost::DoDisplay( + std::ostream& os, const VectorX& vars) const { + return DisplayCost(*this, os, "ExpressionCost", vars); +} + } // namespace solvers } // namespace drake diff --git a/solvers/cost.h b/solvers/cost.h index ee90672b3398..58be46836021 100644 --- a/solvers/cost.h +++ b/solvers/cost.h @@ -10,6 +10,7 @@ #include +#include "drake/solvers/decision_variable.h" #include "drake/solvers/evaluator_base.h" namespace drake { @@ -542,6 +543,48 @@ class PolynomialCost : public EvaluatorCost { } }; +/** + * 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& x, + Eigen::VectorXd* y) const override; + + void DoEval(const Eigen::Ref& x, + AutoDiffVecXd* y) const override; + + void DoEval(const Eigen::Ref>& x, + VectorX* y) const override; + + std::ostream& DoDisplay(std::ostream&, + const VectorX&) const override; + + private: + std::unique_ptr evaluator_; +}; + /** * Converts an input of type @p F to a nonlinear cost. * @tparam FF The forwarded function type (e.g., `const F&, `F&&`, ...). diff --git a/solvers/create_cost.cc b/solvers/create_cost.cc index f613326e25b8..3b803261747e 100644 --- a/solvers/create_cost.cc +++ b/solvers/create_cost.cc @@ -97,10 +97,8 @@ Binding ParsePolynomialCost(const symbolic::Expression& e) { Binding 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(e); + return CreateBinding(cost, cost->vars()); } const symbolic::Polynomial poly{e}; const int total_degree{poly.TotalDegree()}; diff --git a/solvers/mathematical_program.h b/solvers/mathematical_program.h index f9ebef87fb33..f27bc8bae369 100644 --- a/solvers/mathematical_program.h +++ b/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 AddCost(const symbolic::Expression& e); diff --git a/solvers/test/cost_test.cc b/solvers/test/cost_test.cc index 6b360e1355e2..3c81c8a4ba16 100644 --- a/solvers/test/cost_test.cc +++ b/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 x_e(x_test, y_test); + VectorX 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 diff --git a/solvers/test/mathematical_program_test.cc b/solvers/test/mathematical_program_test.cc index e6678e27a7b9..ef3a1d06a5dc 100644 --- a/solvers/test/mathematical_program_test.cc +++ b/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");