Sparse add linear constraint

[AlexandreAmice]

This change is 

[AlexandreAmice]

+@hongkai-dai for feature review. I am requesting help with how to add the python binding.

Reviewable status: LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

[hongkai-dai]

Reviewed 1 of 5 files at r1.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/mathematical_program.h line 1468 at r2 (raw file):

   * of the decision variables (defined in the vars parameter).
   *
   * @pydrake_mkdoc_identifier{4args_A_lb_ub_dense}

The A matrix is sparse but the identifier says it is dense?

---

solvers/mathematical_program.h line 1482 at r2 (raw file):

   * of the decision variables (defined in the vars parameter).
   *
   * @exclude_from_pydrake_mkdoc{Not bound in pydrake.}

Why not to bind this function in pydrake?

---

solvers/mathematical_program.h line 1532 at r2 (raw file):

      const Eigen::Ref<const VectorXDecisionVariable>& vars) {
    return AddLinearConstraint(
        Eigen::Map<const Eigen::MatrixXd>(a.data(), 1, a.size()), Vector1d(lb),

Why making this change?

[AlexandreAmice]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/mathematical_program.h line 1468 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

The A matrix is sparse but the identifier says it is dense?

This was put in the wrong spot. Done.

---

solvers/mathematical_program.h line 1482 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Why not to bind this function in pydrake?

I switched the tags by accident. Good catch.

---

solvers/mathematical_program.h line 1532 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Why making this change?

Eigen needs to be told how to disambiguate between the dense and sparse matrix overload for some reason. My guess is that this has to do with Eigen::Ref. This makes the choice explicit and allows the code to compile

[jwnimmer-tri]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/mathematical_program.h line 1532 at r2 (raw file):

Previously, AlexandreAmice (Alexandre Amice) wrote…

Eigen needs to be told how to disambiguate between the dense and sparse matrix overload for some reason. My guess is that this has to do with Eigen::Ref. This makes the choice explicit and allows the code to compile

If the newly-added ambiguity breaks this call site, it will probably break downstream users' call sites as well. More work is probably needed here. (You could lean on the platform reviewer to help debug it, you don't need to do it on your own.)

[hongkai-dai]

+@jwnimmer-tri for platform review please since you already looked at this PR.

Reviewed 4 of 5 files at r3, 1 of 1 files at r4, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

[AlexandreAmice]

@hongkai-dai could you sign off again as there is the new feature of checking whether the linear constraint already has the dense view.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes

[hongkai-dai]

Reviewed 9 of 13 files at r5, 1 of 1 files at r6.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/constraint.h line 670 at r6 (raw file):

  /** Returns true iff this constraint already has a dense representation, i.e,
   * if GetDenseA() will be cheap. */
  bool HasDenseA() const;

Sorry I don't get why we need this function. Is it only used in the unit test, but not in any other cc file?

[hongkai-dai]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/constraint.h line 670 at r6 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Sorry I don't get why we need this function. Is it only used in the unit test, but not in any other cc file?

Just saw the slack discussion. Retract my comment.

[jwnimmer-tri]

I've posted a few typos below. I'll return in the morning to finish reading the remainder.

Reviewed 11 of 13 files at r5, all commit messages.
Dismissed @hongkai-dai from a discussion.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/mathematical_program.h line 1532 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If the newly-added ambiguity breaks this call site, it will probably break downstream users' call sites as well. More work is probably needed here. (You could lean on the platform reviewer to help debug it, you don't need to do it on your own.)

Done (dismissing Hongkai).

---

solvers/mathematical_program.h line 1773 at r6 (raw file):

   * @endcode
   *
   * @pydrake_mkdoc_identifier{3args_A_b_dense}

BTW This isn't necessarily wrong, but would it be better to name this ...Aeq_beq... to match the arg names instead of ...A_b...?

Ditto just below.

---

solvers/sparse_and_dense_matrix.h line 46 at r6 (raw file):

  // Returns true if the dense matrix has been constructed.
  [[nodiscard]] bool dense_is_constructed() const;

nit For clarity and consistency, we always put the "is" first when that word a verb and the function returns a bool.

Suggestion:

is_dense_constructed

---

bindings/pydrake/solvers/solvers_py_mathematicalprogram.cc line 945 at r6 (raw file):

              &MathematicalProgram::AddLinearEqualityConstraint),
          py::arg("a"), py::arg("beq"), py::arg("vars"),
          doc.MathematicalProgram.AddLinearConstraint.doc_4args_a_lb_ub_vars)

These arg names and the docstring don't seem to line up correctly.

Code quote:

          py::arg("a"), py::arg("beq"), py::arg("vars"),
          doc.MathematicalProgram.AddLinearConstraint.doc_4args_a_lb_ub_vars)

---

bindings/pydrake/solvers/test/evaluators_test.py line 10 at r6 (raw file):

from pydrake.autodiffutils import InitializeAutoDiff
from pydrake.common.test_utilities.deprecation import catch_drake_warnings
import scipy.sparse

nit The scipy import is third-party, so should move up alongside the other third-party import (numpy) around line 5.

(The import typing is in the wrong place too; it should be with the standard library imports (import unittest) near the top. It doesn't matter whether or not you fix that here at the same time. The important thing is that we don't make anything worse in this PR.)

---

bindings/pydrake/solvers/test/evaluators_test.py line 107 at r6 (raw file):

    def test_linear_constraint(self):
        A_sparse = scipy.sparse.csc_matrix(
            (np.array([2, 1., 3]), np.array([0, 1, 0]),

typo

(https://drake.mit.edu/styleguide/cppguide.html#Floating_Literals)

Ditto on line 783 in mathematicalprogram_test.py.

Suggestion:

1

[AlexandreAmice]

This PR is turning into quite the hunt for spurious uses of GetDenseA(). I have added some TODOs to purge the use of this throughout the code base, but I am wondering if there is a way to discourage the use of this method?

Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/constraint.h line 670 at r7 (raw file):

  /** Returns true iff this constraint already has a dense representation, i.e,
   * if GetDenseA() will be cheap. */
  bool is_dense_A_constructed() const;

@jwnimmer-tri I changed the name here as it is more accurate.

---

bindings/pydrake/solvers/solvers_py_mathematicalprogram.cc line 945 at r6 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

These arg names and the docstring don't seem to line up correctly.

Done.

[jwnimmer-tri]

I am wondering if there is a way to discourage the use of this method?

I think having tests that catch the bugs is a pretty good way, actually. Or, a grep for non-test uses of GetDenseA is probably also not too bad. Looks like only about a dozen non-test calls. I guess that's what you've been doing already.

The only other option I can think of is to rename GetDenseA to something more scary (with deprecation), so that it's apparent at call sites that its not supposed to be used. Part of the problem is just inertia -- for a long time there was only the dense function, so most code was using that function out of habit. Once all of the TODOs are finished, I think there's a good chance that new code ends up using sparse, because the developer will copy-paste from the existing uses that will all be sparse by then.

Reviewed 16 of 18 files at r7, all commit messages.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/constraint.h line 670 at r7 (raw file):

Previously, AlexandreAmice (Alexandre Amice) wrote…

@jwnimmer-tri I changed the name here as it is more accurate.

SGTM

[jwnimmer-tri]

Reviewed 2 of 18 files at r7.
Reviewable status: 3 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @AlexandreAmice)

---

solvers/test/mathematical_program_test.cc line 807 at r7 (raw file):

}

// Return True if all the elements of the matrix m1 and m2 are EqualTo as

nit GSG https://drake.mit.edu/styleguide/cppguide.html#Function_Comments

Suggestion:

Returns

---

solvers/test/mathematical_program_test.cc line 1284 at r7 (raw file):

  std::vector<Eigen::Triplet<double>> A_triplet_list;
  A_triplet_list.reserve(3 * (n - 2) + 4);
  double ctr = 1;

nit Do not abbreviate by deleting letters within a word.

-- https://drake.mit.edu/styleguide/cppguide.html#General_Naming_Rules

Ditto throughout the new code below.

Suggestion:

count

---

solvers/test/mathematical_program_test.cc line 1309 at r7 (raw file):

  const MatrixX<Expression> Ax{(constraint_ptr->GetDenseA() * var_vec)};
  const VectorX<Expression> lb_in_ctr{constraint_ptr->lower_bound()};
  const VectorX<Expression> ub_in_ctr{constraint_ptr->upper_bound()};

nit I don't know what in_ctr is trying to say here? It is supposed to be "in constraint"?

Ditto in the similar code down below.

[AlexandreAmice]

+(status: squashing now)

Reviewable status: 1 unresolved discussion, missing label for release notes (waiting on @AlexandreAmice)

---

solvers/test/mathematical_program_test.cc line 1309 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit I don't know what in_ctr is trying to say here? It is supposed to be "in constraint"?

Ditto in the similar code down below.

Changed them all in the file.

[jwnimmer-tri]

Reviewed 1 of 1 files at r8, all commit messages.
Reviewable status: missing label for release notes (waiting on @AlexandreAmice)