Add Expression constraint to handle general (nonlinear) constraints s…

…pecified by symbolic Expressions

bindings/pydrake/solvers/mathematicalprogram_py.cc

@@ -196,6 +196,13 @@ PYBIND11_MODULE(_mathematicalprogram_py, m) {
                const Eigen::Ref<MatrixX<symbolic::Variable>>& vars) {
              return self->AddBoundingBoxConstraint(lb, ub, vars);
       })
+      .def("AddConstraint",
+           static_cast<Binding<Constraint> (MathematicalProgram::*)(
+               const Expression&, double, double)>(
+               &MathematicalProgram::AddConstraint))
+      .def("AddConstraint",
+           static_cast<Binding<Constraint> (MathematicalProgram::*)(
+               const Formula&)>(&MathematicalProgram::AddConstraint))
       .def("AddLinearConstraint",
            static_cast<Binding<LinearConstraint> (MathematicalProgram::*)(
                const Expression&, double, double)>(
@@ -325,6 +332,7 @@ PYBIND11_MODULE(_mathematicalprogram_py, m) {
              std::shared_ptr<LinearComplementarityConstraint>>(
       m, "LinearComplementarityConstraint");
 
+  RegisterBinding<Constraint>(&m, &prog_cls, "Constraint");
   RegisterBinding<LinearConstraint>(&m, &prog_cls, "LinearConstraint");
   RegisterBinding<LinearEqualityConstraint>(&m, &prog_cls,
                                             "LinearEqualityConstraint");

bindings/pydrake/solvers/test/mathematicalprogram_test.py

@@ -73,8 +73,8 @@ def test_qp(self):
     def test_symbolic_qp(self):
         prog = mp.MathematicalProgram()
         x = prog.NewContinuousVariables(2, "x")
-        prog.AddLinearConstraint(x[0] >= 1)
-        prog.AddLinearConstraint(x[1] >= 1)
+        prog.AddConstraint(x[0], 1., 100.)
+        prog.AddConstraint(x[1] >= 1)
         prog.AddQuadraticCost(x[0]**2 + x[1]**2)
         result = prog.Solve()
         self.assertEqual(result, mp.SolutionResult.kSolutionFound)

common/symbolic_expression.cc

@@ -855,6 +855,18 @@ MatrixX<Expression> Jacobian(const Eigen::Ref<const VectorX<Expression>>& f,
                              const Eigen::Ref<const VectorX<Variable>>& vars) {
   return Jacobian(f, vector<Variable>(vars.data(), vars.data() + vars.size()));
 }
+
+Variables GetDistinctVariables(const Eigen::Ref<const MatrixX<Expression>>& v) {
+  Variables vars{};
+  // Note: Default storage order for Eigen is column-major.
+  for (int j = 0; j < v.cols(); j++) {
+    for (int i = 0; i < v.rows(); i++) {
+      vars.insert(v(i, j).GetVariables());
+    }
+  }
+  return vars;
+}
+
 }  // namespace symbolic
 
 double ExtractDoubleOrThrow(const symbolic::Expression& e) {

common/symbolic_expression.h

@@ -899,6 +899,9 @@ MatrixX<Expression> Jacobian(const Eigen::Ref<const VectorX<Expression>>& f,
 MatrixX<Expression> Jacobian(const Eigen::Ref<const VectorX<Expression>>& f,
                              const Eigen::Ref<const VectorX<Variable>>& vars);
 
+/// Returns the distinct variables in the matrix of expressions.
+Variables GetDistinctVariables(const Eigen::Ref<const MatrixX<Expression>>& v);
+
 /// Checks if two Eigen::Matrix<Expression> @p m1 and @p m2 are structurally
 /// equal. That is, it returns true if and only if `m1(i, j)` is structurally
 /// equal to `m2(i, j)` for all `i`, `j`.

common/test/symbolic_expression_test.cc

@@ -1899,6 +1899,17 @@ TEST_F(SymbolicExpressionTest, Jacobian) {
   EXPECT_EQ(J2(1), J1(1));
 }
 
+TEST_F(SymbolicExpressionTest, GetDistinctVariables) {
+  EXPECT_EQ(GetDistinctVariables(Vector1<Expression>{x_plus_y_}),
+            Variables({var_x_, var_y_}));
+  EXPECT_EQ(GetDistinctVariables(Vector1<Expression>{x_plus_z_}),
+            Variables({var_x_, var_z_}));
+  EXPECT_EQ(GetDistinctVariables(Vector2<Expression>{x_plus_y_, x_plus_z_}),
+            Variables({var_x_, var_y_, var_z_}));
+  EXPECT_EQ(GetDistinctVariables(RowVector2<Expression>{x_plus_z_, e_cos_}),
+            Variables({var_x_, var_z_}));
+}
+
 }  // namespace
 }  // namespace symbolic
 }  // namespace drake

solvers/BUILD.bazel

@@ -38,10 +38,13 @@ drake_cc_library(
     srcs = ["constraint.cc"],
     hdrs = ["constraint.h"],
     deps = [
+        ":decision_variable",
         ":evaluator_base",
+        ":symbolic_extraction",
         "//common:autodiff",
         "//common:essential",
         "//common:polynomial",
+        "//common:symbolic",
         "//math:matrix_util",
     ],
 )

solvers/binding.h

@@ -85,6 +85,13 @@ Binding<C> CreateBinding(const std::shared_ptr<C>& c, Args&&... args) {
   return Binding<C>(c, std::forward<Args>(args)...);
 }
 
+template <typename To, typename From>
+Binding<To> BindingDynamicCast(const Binding<From>& binding) {
+  auto constraint = std::dynamic_pointer_cast<To>(binding.evaluator());
+  DRAKE_DEMAND(constraint != nullptr);
+  return Binding<To>(constraint, binding.variables());
+}
+
 }  // namespace internal
 
 }  // namespace solvers

solvers/constraint.cc

@@ -1,8 +1,10 @@
 #include "drake/solvers/constraint.h"
 
 #include <cmath>
+#include <unordered_map>
 
 #include "drake/math/matrix_util.h"
+#include "drake/solvers/symbolic_extraction.h"
 
 using std::abs;
 
@@ -166,5 +168,72 @@ LinearMatrixInequalityConstraint::LinearMatrixInequalityConstraint(
     DRAKE_ASSERT(math::IsSymmetric(Fi, symmetry_tolerance));
   }
 }
+
+ExpressionConstraint::ExpressionConstraint(
+    const Eigen::Ref<const VectorX<symbolic::Expression>>& v,
+    const Eigen::Ref<const Eigen::VectorXd>& lb,
+    const Eigen::Ref<const Eigen::VectorXd>& ub)
+    : Constraint(v.rows(), GetDistinctVariables(v).size(), lb, ub),
+      expressions_(v) {
+  // Setup map_var_to_index_ and vars_ so that
+  //   map_var_to_index_[vars_(i).get_id()] = i.
+  for (int i = 0; i < expressions_.size(); ++i) {
+    internal::ExtractAndAppendVariablesFromExpression(expressions_(i), &vars_,
+                                                      &map_var_to_index_);
+  }
+
+  derivatives_ = symbolic::Jacobian(expressions_, vars_);
+
+  // Setup the environment.
+  for (int i = 0; i < vars_.size(); i++) {
+    environment_.insert(vars_[i], 0.0);
+  }
+}
+
+void ExpressionConstraint::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(num_constraints());
+  for (int i = 0; i < num_constraints(); i++) {
+    y[i] = expressions_[i].Evaluate(environment_);
+  }
+}
+
+void ExpressionConstraint::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(num_constraints());
+  Eigen::VectorXd dyidx(x.size());
+  for (int i = 0; i < num_constraints(); i++) {
+    y[i].value() = expressions_[i].Evaluate(environment_);
+    for (int k = 0; k < x.size(); k++) {
+      dyidx[k] = derivatives_(i, k).Evaluate(environment_);
+    }
+
+    y[i].derivatives().resize(x(0).derivatives().size());
+    for (int j = 0; j < x(0).derivatives().size(); j++) {
+      y[i].derivatives()[j] = 0.0;
+      for (int k = 0; k < x.size(); k++) {
+        y[i].derivatives()[j] += dyidx[k] * x(k).derivatives()[j];
+      }
+    }
+  }
+}
+
 }  // namespace solvers
 }  // namespace drake

solvers/constraint.h

@@ -6,6 +6,7 @@
 #include <memory>
 #include <stdexcept>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -16,6 +17,8 @@
 #include "drake/common/drake_copyable.h"
 #include "drake/common/eigen_types.h"
 #include "drake/common/polynomial.h"
+#include "drake/common/symbolic.h"
+#include "drake/solvers/decision_variable.h"
 #include "drake/solvers/evaluator_base.h"
 #include "drake/solvers/function.h"
 
@@ -779,5 +782,48 @@ class LinearMatrixInequalityConstraint : public Constraint {
   const int matrix_rows_{};
 };
 
+
+/**
+ * Impose a generic (potentially nonlinear) constraint represented as a
+ * vector of symbolic Expression.  Expression::Evaluate is called on every
+ * constraint evaluation.
+ *
+ * Uses symbolic::Jacobian to provide the gradients to the AutoDiff method.
+ */
+class ExpressionConstraint : public Constraint {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ExpressionConstraint)
+
+  ExpressionConstraint(const Eigen::Ref<const VectorX<symbolic::Expression>>& v,
+                       const Eigen::Ref<const Eigen::VectorXd>& lb,
+                       const Eigen::Ref<const Eigen::VectorXd>& ub);
+
+  /**
+   * @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_; }
+
+ 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;
+
+ private:
+  VectorX<symbolic::Expression> expressions_{0};
+  MatrixX<symbolic::Expression> derivatives_{0, 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_;
+};
+
 }  // namespace solvers
 }  // namespace drake