Sparse add linear constraint

bindings/pydrake/solvers/solvers_py_evaluators.cc

@@ -261,6 +261,8 @@ void BindEvaluatorsAndBindings(py::module m) {
           doc.LinearConstraint.UpdateCoefficients.doc_sparse_A)
       .def("RemoveTinyCoefficient", &LinearConstraint::RemoveTinyCoefficient,
           py::arg("tol"), doc.LinearConstraint.RemoveTinyCoefficient.doc)
+      .def("is_dense_A_constructed", &LinearConstraint::is_dense_A_constructed,
+          doc.LinearConstraint.is_dense_A_constructed.doc)
       .def(
           "UpdateLowerBound",
           [](LinearConstraint& self, const Eigen::VectorXd& new_lb) {

bindings/pydrake/solvers/solvers_py_mathematicalprogram.cc

@@ -883,14 +883,23 @@ void BindMathematicalProgram(py::module m) {
               const Eigen::Ref<const VectorXDecisionVariable>&)>(
               &MathematicalProgram::AddLinearConstraint),
           py::arg("A"), py::arg("lb"), py::arg("ub"), py::arg("vars"),
-          doc.MathematicalProgram.AddLinearConstraint.doc_4args_A_lb_ub_vars)
+          doc.MathematicalProgram.AddLinearConstraint.doc_4args_A_lb_ub_dense)
       .def("AddLinearConstraint",
           static_cast<Binding<LinearConstraint> (MathematicalProgram::*)(
               const Eigen::Ref<const Eigen::RowVectorXd>&, double, double,
               const Eigen::Ref<const VectorXDecisionVariable>&)>(
               &MathematicalProgram::AddLinearConstraint),
           py::arg("a"), py::arg("lb"), py::arg("ub"), py::arg("vars"),
           doc.MathematicalProgram.AddLinearConstraint.doc_4args_a_lb_ub_vars)
+      .def("AddLinearConstraint",
+          static_cast<Binding<LinearConstraint> (MathematicalProgram::*)(
+              const Eigen::SparseMatrix<double>&,
+              const Eigen::Ref<const Eigen::VectorXd>&,
+              const Eigen::Ref<const Eigen::VectorXd>&,
+              const Eigen::Ref<const VectorXDecisionVariable>&)>(
+              &MathematicalProgram::AddLinearConstraint),
+          py::arg("A"), py::arg("lb"), py::arg("ub"), py::arg("vars"),
+          doc.MathematicalProgram.AddLinearConstraint.doc_4args_A_lb_ub_sparse)
       .def("AddLinearConstraint",
           static_cast<Binding<LinearConstraint> (MathematicalProgram::*)(
               const Expression&, double, double)>(
@@ -925,7 +934,25 @@ void BindMathematicalProgram(py::module m) {
               &MathematicalProgram::AddLinearEqualityConstraint),
           py::arg("Aeq"), py::arg("beq"), py::arg("vars"),
           doc.MathematicalProgram.AddLinearEqualityConstraint
-              .doc_3args_Aeq_beq_vars)
+              .doc_3args_Aeq_beq_dense)
+      .def("AddLinearEqualityConstraint",
+          static_cast<Binding<LinearEqualityConstraint> (
+              MathematicalProgram::*)(
+              const Eigen::Ref<const Eigen::RowVectorXd>&, double,
+              const Eigen::Ref<const VectorXDecisionVariable>&)>(
+              &MathematicalProgram::AddLinearEqualityConstraint),
+          py::arg("a"), py::arg("beq"), py::arg("vars"),
+          doc.MathematicalProgram.AddLinearEqualityConstraint
+              .doc_3args_a_beq_vars)
+      .def("AddLinearEqualityConstraint",
+          static_cast<Binding<LinearEqualityConstraint> (
+              MathematicalProgram::*)(const Eigen::SparseMatrix<double>&,
+              const Eigen::Ref<const Eigen::VectorXd>&,
+              const Eigen::Ref<const VectorXDecisionVariable>&)>(
+              &MathematicalProgram::AddLinearEqualityConstraint),
+          py::arg("Aeq"), py::arg("beq"), py::arg("vars"),
+          doc.MathematicalProgram.AddLinearEqualityConstraint
+              .doc_3args_Aeq_beq_sparse)
       .def("AddLinearEqualityConstraint",
           static_cast<Binding<LinearEqualityConstraint> (
               MathematicalProgram::*)(const Expression&, double)>(

bindings/pydrake/solvers/test/evaluators_test.py

@@ -1,7 +1,8 @@
 import unittest
+import typing
 
 import numpy as np
-import typing
+import scipy.sparse
 
 import pydrake.solvers as mp
 import pydrake.symbolic as sym
@@ -101,6 +102,39 @@ def test_bounding_box_constraint(self):
         np.testing.assert_array_equal(
             constraint.upper_bound(), np.array([2., 3.]))
 
+    def test_linear_constraint(self):
+        A_sparse = scipy.sparse.csc_matrix(
+            (np.array([2, 1, 3]), np.array([0, 1, 0]),
+             np.array([0, 2, 2, 3])), shape=(2, 2))
+        lb = -np.ones(2)
+        ub = np.ones(2)
+
+        constraints = []
+        constraints.append(mp.LinearConstraint(A=np.eye(2), lb=lb, ub=ub))
+        self.assertTrue(constraints[-1].is_dense_A_constructed())
+        constraints.append(mp.LinearConstraint(A=A_sparse, lb=lb, ub=ub))
+        self.assertFalse(constraints[-1].is_dense_A_constructed())
+
+        for c in constraints:
+            self.assertEqual(c.GetDenseA().shape[1], 2)
+            self.assertEqual(c.get_sparse_A().shape[1], 2)
+            r = c.GetDenseA().shape[0]
+            new_A = np.ones_like(c.GetDenseA())
+            new_A[1, 1] = 1e-20
+
+            c.UpdateCoefficients(new_A=new_A,
+                                 new_lb=np.ones(r),
+                                 new_ub=np.ones(r))
+            c.RemoveTinyCoefficient(tol=1e-10)
+            self.assertEqual(c.GetDenseA()[1, 1], 0)
+            c.UpdateCoefficients(new_A=c.get_sparse_A(),
+                                 new_lb=np.ones(r),
+                                 new_ub=np.ones(r))
+
+            c.UpdateLowerBound(new_lb=-2*np.ones(r))
+            c.UpdateUpperBound(new_ub=2*np.ones(r))
+            c.set_bounds(new_lb=-3*np.ones(r), new_ub=3*np.ones(r))
+
     def test_quadratic_constraint(self):
         hessian_type = mp.QuadraticConstraint.HessianType.kPositiveSemidefinite
         constraint = mp.QuadraticConstraint(

bindings/pydrake/solvers/test/mathematicalprogram_test.py

@@ -778,21 +778,38 @@ def test_linear_constraints(self):
         x = prog.NewContinuousVariables(2, 'x')
         lb = [0., 0.]
         ub = [1., 1.]
+
+        A_sparse = scipy.sparse.csc_matrix(
+            (np.array([2, 1, 3]), np.array([0, 1, 0]),
+             np.array([0, 2, 2, 3])), shape=(2, 2))
+
         prog.AddBoundingBoxConstraint(lb, ub, x)
         prog.AddBoundingBoxConstraint(0., 1., x[0])
         prog.AddBoundingBoxConstraint(0., 1., x)
         prog.AddLinearConstraint(A=np.eye(2), lb=np.zeros(2), ub=np.ones(2),
                                  vars=x)
+        c1 = prog.AddLinearConstraint(A=A_sparse,
+                                      lb=np.zeros(2),
+                                      ub=np.ones(2),
+                                      vars=x)
+        # Ensure that the sparse version of the binding has been called.
+        self.assertFalse(c1.evaluator().is_dense_A_constructed())
         prog.AddLinearConstraint(a=[1, 1], lb=0, ub=0, vars=x)
         prog.AddLinearConstraint(e=x[0], lb=0, ub=1)
         prog.AddLinearConstraint(v=x, lb=[0, 0], ub=[1, 1])
         prog.AddLinearConstraint(f=(x[0] == 0))
 
-        prog.AddLinearEqualityConstraint(np.eye(2), np.zeros(2), x)
-        prog.AddLinearEqualityConstraint(x[0] == 1)
-        prog.AddLinearEqualityConstraint(x[0] + x[1], 1)
+        prog.AddLinearEqualityConstraint(Aeq=np.eye(2), beq=np.zeros(2),
+                                         vars=x)
+        c2 = prog.AddLinearEqualityConstraint(Aeq=A_sparse, beq=np.zeros(2),
+                                              vars=x)
+        # Ensure that the sparse version of the binding has been called.
+        self.assertFalse(c2.evaluator().is_dense_A_constructed())
+        prog.AddLinearEqualityConstraint(a=[1, 1], beq=0, vars=x)
+        prog.AddLinearEqualityConstraint(f=x[0] == 1)
+        prog.AddLinearEqualityConstraint(e=x[0] + x[1], b=1)
         prog.AddLinearEqualityConstraint(
-            2 * x[:2] + np.array([0, 1]), np.array([3, 2]))
+            v=2 * x[:2] + np.array([0, 1]), b=np.array([3, 2]))
 
     def test_constraint_set_bounds(self):
         prog = mp.MathematicalProgram()

geometry/optimization/graph_of_convex_sets.cc

@@ -726,6 +726,7 @@ void GraphOfConvexSets::AddPerspectiveConstraint(
       // Then do nothing.
     } else {
       // Need to go constraint by constraint.
+      // TODO(Alexandre.Amice) make this only access the sparse matrix.
       const Eigen::MatrixXd& A = lc->GetDenseA();
       RowVectorXd a(vars.size());
       for (int i = 0; i < A.rows(); ++i) {

geometry/optimization/iris.cc

@@ -564,9 +564,9 @@ HPolyhedron IrisInConfigurationSpace(const MultibodyPlant<double>& plant,
     };
     auto HandleLinearConstraints = [&](const auto& bindings) {
       for (const auto& binding : bindings) {
-        AddConstraint(binding.evaluator()->GetDenseA(),
+        AddConstraint(binding.evaluator()->get_sparse_A(),
                       binding.evaluator()->upper_bound(), binding.variables());
-        AddConstraint(-binding.evaluator()->GetDenseA(),
+        AddConstraint(-binding.evaluator()->get_sparse_A(),
                       -binding.evaluator()->lower_bound(), binding.variables());
         auto pos = std::find(additional_constraint_bindings.begin(),
                              additional_constraint_bindings.end(), binding);

solvers/constraint.cc

@@ -468,6 +468,10 @@ void LinearConstraint::RemoveTinyCoefficient(double tol) {
   UpdateCoefficients(new_A, lower_bound(), upper_bound());
 }
 
+bool LinearConstraint::is_dense_A_constructed() const {
+  return A_.is_dense_constructed();
+}
+
 template <typename DerivedX, typename ScalarY>
 void LinearConstraint::DoEvalGeneric(const Eigen::MatrixBase<DerivedX>& x,
                                      VectorX<ScalarY>* y) const {

solvers/constraint.h

@@ -665,6 +665,10 @@ class LinearConstraint : public Constraint {
    */
   void RemoveTinyCoefficient(double tol);
 
+  /** Returns true iff this constraint already has a dense representation, i.e,
+   * if GetDenseA() will be cheap. */
+  bool is_dense_A_constructed() const;
+
   using Constraint::set_bounds;
   using Constraint::UpdateLowerBound;
   using Constraint::UpdateUpperBound;
@@ -707,15 +711,17 @@ class LinearEqualityConstraint : public LinearConstraint {
    */
   LinearEqualityConstraint(const Eigen::Ref<const Eigen::MatrixXd>& Aeq,
                            const Eigen::Ref<const Eigen::VectorXd>& beq)
-      : LinearConstraint(Aeq, beq, beq) {}
+      : LinearConstraint(Aeq, beq, beq) {
+  }
 
   /**
    * Overloads the constructor with a sparse matrix Aeq.
    * @pydrake_mkdoc_identifier{sparse_Aeq}
    */
   LinearEqualityConstraint(const Eigen::SparseMatrix<double>& Aeq,
                            const Eigen::Ref<const Eigen::VectorXd>& beq)
-      : LinearConstraint(Aeq, beq, beq) {}
+      : LinearConstraint(Aeq, beq, beq) {
+  }
 
   /**
    * Constructs the linear equality constraint a.dot(x) = beq

solvers/ipopt_solver.cc

@@ -229,7 +229,7 @@ size_t EvaluateConstraint(const MathematicalProgram& prog,
   // constraints.
   if constexpr (std::is_same_v<ConstraintType, LinearEqualityConstraint> ||
                 std::is_same_v<ConstraintType, LinearConstraint>) {
-    auto A = static_cast<ConstraintType*>(c)->GetDenseA();
+    auto A = static_cast<ConstraintType*>(c)->get_sparse_A();
     // Verify that A has the proper size.
     DRAKE_ASSERT(A.rows() == c->num_constraints());
     DRAKE_ASSERT(A.cols() == binding.variables().rows());

solvers/mathematical_program.cc

@@ -860,7 +860,7 @@ Binding<LinearConstraint> MathematicalProgram::AddConstraint(
   } else {
     // TODO(eric.cousineau): This is a good assertion... But seems out of place,
     // possibly redundant w.r.t. the binding infrastructure.
-    DRAKE_ASSERT(binding.evaluator()->GetDenseA().cols() ==
+    DRAKE_ASSERT(binding.evaluator()->get_sparse_A().cols() ==
                  static_cast<int>(binding.GetNumElements()));
     if (!CheckBinding(binding)) {
       return binding;
@@ -879,9 +879,17 @@ Binding<LinearConstraint> MathematicalProgram::AddLinearConstraint(
   return AddConstraint(make_shared<LinearConstraint>(A, lb, ub), vars);
 }
 
+Binding<LinearConstraint> MathematicalProgram::AddLinearConstraint(
+    const Eigen::SparseMatrix<double>& A,
+    const Eigen::Ref<const Eigen::VectorXd>& lb,
+    const Eigen::Ref<const Eigen::VectorXd>& ub,
+    const Eigen::Ref<const VectorXDecisionVariable>& vars) {
+  return AddConstraint(make_shared<LinearConstraint>(A, lb, ub), vars);
+}
+
 Binding<LinearEqualityConstraint> MathematicalProgram::AddConstraint(
     const Binding<LinearEqualityConstraint>& binding) {
-  DRAKE_ASSERT(binding.evaluator()->GetDenseA().cols() ==
+  DRAKE_ASSERT(binding.evaluator()->get_sparse_A().cols() ==
                static_cast<int>(binding.GetNumElements()));
   if (!CheckBinding(binding)) {
     return binding;
@@ -910,6 +918,14 @@ MathematicalProgram::AddLinearEqualityConstraint(
   return AddConstraint(make_shared<LinearEqualityConstraint>(Aeq, beq), vars);
 }
 
+Binding<LinearEqualityConstraint>
+MathematicalProgram::AddLinearEqualityConstraint(
+    const Eigen::SparseMatrix<double>& Aeq,
+    const Eigen::Ref<const Eigen::VectorXd>& beq,
+    const Eigen::Ref<const VectorXDecisionVariable>& vars) {
+  return AddConstraint(make_shared<LinearEqualityConstraint>(Aeq, beq), vars);
+}
+
 Binding<BoundingBoxConstraint> MathematicalProgram::AddConstraint(
     const Binding<BoundingBoxConstraint>& binding) {
   if (!CheckBinding(binding)) {

solvers/mathematical_program.h

@@ -18,6 +18,7 @@
 #include <vector>
 
 #include <Eigen/Core>
+#include <Eigen/SparseCore>
 
 #include "drake/common/autodiff.h"
 #include "drake/common/drake_assert.h"
@@ -1448,6 +1449,8 @@ class MathematicalProgram {
   /**
    * Adds linear constraints referencing potentially a subset
    * of the decision variables (defined in the vars parameter).
+   *
+   * @exclude_from_pydrake_mkdoc{Not bound in pydrake.}
    */
   Binding<LinearConstraint> AddLinearConstraint(
       const Eigen::Ref<const Eigen::MatrixXd>& A,
@@ -1457,16 +1460,44 @@ class MathematicalProgram {
     return AddLinearConstraint(A, lb, ub, ConcatenateVariableRefList(vars));
   }
 
+  /**
+   * Adds sparse linear constraints referencing potentially a subset
+   * of the decision variables (defined in the vars parameter).
+   *
+   * @exclude_from_pydrake_mkdoc{Not bound in pydrake.}
+   */
+  Binding<LinearConstraint> AddLinearConstraint(
+      const Eigen::SparseMatrix<double>& A,
+      const Eigen::Ref<const Eigen::VectorXd>& lb,
+      const Eigen::Ref<const Eigen::VectorXd>& ub,
+      const VariableRefList& vars) {
+    return AddLinearConstraint(A, lb, ub, ConcatenateVariableRefList(vars));
+  }
+
   /**
    * Adds linear constraints referencing potentially a subset
    * of the decision variables (defined in the vars parameter).
+   *
+   * @pydrake_mkdoc_identifier{4args_A_lb_ub_dense}
    */
   Binding<LinearConstraint> AddLinearConstraint(
       const Eigen::Ref<const Eigen::MatrixXd>& A,
       const Eigen::Ref<const Eigen::VectorXd>& lb,
       const Eigen::Ref<const Eigen::VectorXd>& ub,
       const Eigen::Ref<const VectorXDecisionVariable>& vars);
 
+  /**
+   * Adds sparse linear constraints referencing potentially a subset
+   * of the decision variables (defined in the vars parameter).
+   *
+   * @pydrake_mkdoc_identifier{4args_A_lb_ub_sparse}
+   */
+  Binding<LinearConstraint> AddLinearConstraint(
+      const Eigen::SparseMatrix<double>& A,
+      const Eigen::Ref<const Eigen::VectorXd>& lb,
+      const Eigen::Ref<const Eigen::VectorXd>& ub,
+      const Eigen::Ref<const VectorXDecisionVariable>& vars);
+
   /**
    * Adds one row of linear constraint referencing potentially a
    * subset of the decision variables (defined in the vars parameter).
@@ -1706,6 +1737,20 @@ class MathematicalProgram {
                                        ConcatenateVariableRefList(vars));
   }
 
+  /** AddLinearEqualityConstraint
+   *
+   * Adds linear equality constraints referencing potentially a subset of
+   * the decision variables using a sparse A matrix.
+   * @exclude_from_pydrake_mkdoc{Not bound in pydrake.}
+   */
+  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
+      const Eigen::SparseMatrix<double>& Aeq,
+      const Eigen::Ref<const Eigen::VectorXd>& beq,
+      const VariableRefList& vars) {
+    return AddLinearEqualityConstraint(Aeq, beq,
+                                       ConcatenateVariableRefList(vars));
+  }
+
   /** AddLinearEqualityConstraint
    *
    * Adds linear equality constraints referencing potentially a subset of
@@ -1724,12 +1769,25 @@ class MathematicalProgram {
    *   //  x(0) +  x(1) = 3
    *   prog.AddLinearEqualityConstraint(Aeq, beq, x.head<2>());
    * @endcode
+   *
+   * @pydrake_mkdoc_identifier{3args_Aeq_beq_dense}
    */
   Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
       const Eigen::Ref<const Eigen::MatrixXd>& Aeq,
       const Eigen::Ref<const Eigen::VectorXd>& beq,
       const Eigen::Ref<const VectorXDecisionVariable>& vars);
 
+  /** AddLinearEqualityConstraint
+   *
+   * Adds linear equality constraints referencing potentially a subset of
+   * the decision variables using a sparse A matrix.
+   * @pydrake_mkdoc_identifier{3args_Aeq_beq_sparse}
+   */
+  Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
+      const Eigen::SparseMatrix<double>& Aeq,
+      const Eigen::Ref<const Eigen::VectorXd>& beq,
+      const Eigen::Ref<const VectorXDecisionVariable>& vars);
+
   /**
    * Adds one row of linear equality constraint referencing potentially a subset
    * of decision variables.
@@ -1745,8 +1803,8 @@ class MathematicalProgram {
   Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
       const Eigen::Ref<const Eigen::RowVectorXd>& a, double beq,
       const VariableRefList& vars) {
-    return AddConstraint(std::make_shared<LinearEqualityConstraint>(a, beq),
-                         ConcatenateVariableRefList(vars));
+    return AddLinearEqualityConstraint(a, beq,
+                                       ConcatenateVariableRefList(vars));
   }
 
   /**
@@ -1758,8 +1816,6 @@ class MathematicalProgram {
    * @param a A row vector.
    * @param beq A scalar.
    * @param vars The decision variables on which the constraint is imposed.
-   *
-   * @exclude_from_pydrake_mkdoc{Not bound in pydrake.}
    */
   Binding<LinearEqualityConstraint> AddLinearEqualityConstraint(
       const Eigen::Ref<const Eigen::RowVectorXd>& a, double beq,