diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index eea735c7672e..f168ef36ba64 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 = [ + "//common/symbolic:polynomial", "//math:gradient", ], ) diff --git a/solvers/cost.cc b/solvers/cost.cc index 97b21b0fc3d3..a56b05af94d2 100644 --- a/solvers/cost.cc +++ b/solvers/cost.cc @@ -2,6 +2,7 @@ #include +#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& 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& 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>& 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); + } +} + +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..b1110a34546f 100644 --- a/solvers/cost.h +++ b/solvers/cost.h @@ -5,11 +5,13 @@ #include #include #include +#include #include #include #include +#include "drake/solvers/decision_variable.h" #include "drake/solvers/evaluator_base.h" namespace drake { @@ -542,6 +544,58 @@ 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 vars_; } + + /** @return the symbolic expression. */ + const symbolic::Expression& expression() const { + return expression_; + } + + 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: + 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_; +}; + /** * 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/test/cost_test.cc b/solvers/test/cost_test.cc index 6b360e1355e2..740e0d71f7b3 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(fmt::format("{}", 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..b9bfac21aafa 100644 --- a/solvers/test/mathematical_program_test.cc +++ b/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");