Add support for parsing symbolic L2 norm costs

[RussTedrake]

In a recent underactuated pset, we found that lots of people wanted to pass e.g. np.linalg.norm(x1-x2) to the AddCost methods, and were stymied by a lack of support there. Asking them to instead suddenly understand how to create an L2NormCost, create the binding, and pass that to (GCS's version of) AddCost was too much.

This PR adds DecomposeL2NormExpression, and uses it in prog.AddCost(expression). Consistent with the other Add*Cost methods, it also adds prog.AddL2NormCost(expression), with all of the specific tolerance arguments.

+@hongkai-dai for feature review, please.

---

This change is 

[hongkai-dai]

Reviewed 2 of 14 files at r1.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers

---

common/symbolic/decompose.cc line 393 at r1 (raw file):

    return {false, A, b, vars};
  }
  b = A.transpose().colPivHouseholderQr().solve(0.5 * r);

Here we solve Aᵀb = 0.5r, but this linear system of equation might not have a solution (r might not be in the range of Aᵀ, if some eigen value of Q is exactly 0`.

One example is that the quadratic expression is x²+x+y. So I would think we need to check if Aᵀb = 0.5r is valid.

[RussTedrake]

Reviewable status: 1 unresolved discussion, LGTM missing from assignee hongkai-dai, needs platform reviewer assigned, needs at least two assigned reviewers

---

common/symbolic/decompose.cc line 393 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Here we solve Aᵀb = 0.5r, but this linear system of equation might not have a solution (r might not be in the range of Aᵀ, if some eigen value of Q is exactly 0`.

One example is that the quadratic expression is x²+x+y. So I would think we need to check if Aᵀb = 0.5r is valid.

Done. Good catch. (I shouldn't have missed that!) Thanks.

[hongkai-dai]

+@rpoyner-tri for platform review please, thanks!

Reviewed 3 of 14 files at r1, 10 of 10 files at r2, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee rpoyner-tri(platform)

---

common/symbolic/decompose.h line 172 at r2 (raw file):

negative eigenvalues less than this threshold are considered to be not positive
semidefinite, and will cause the decomposition to fail.
@param coefficient_tolerance The absolute tolerance for checking that that the

nit, typo, duplicated "that".

---

common/symbolic/decompose.cc line 371 at r2 (raw file):

  }
  auto e_extracted = ExtractVariablesFromExpression(e);
  vars = e_extracted.first;

nit, consider using std::move here to avoid copying.

[hongkai-dai]

-@rpoyner-tri for platform review. +@jwnimmer-tri for platform review please, thanks!

Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform)

[jwnimmer-tri]

Reviewed 4 of 14 files at r1, 10 of 10 files at r2, all commit messages.
Reviewable status: 14 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform)

---

common/symbolic/BUILD.bazel line 187 at r2 (raw file):

        ":expression",
        "//math:matrix_util",
        "//math:quadratic_form",

Working

This is a newly dependency cycle between common <=> math. I need to give it some consideration to see if we need some kind of surgery to avoid it.

(If there is a sensible way to rewrite the code to not have such a cycle, that would probably be better.)

---

common/symbolic/decompose.h line 172 at r2 (raw file):

negative eigenvalues less than this threshold are considered to be not positive
semidefinite, and will cause the decomposition to fail.
@param coefficient_tolerance The absolute tolerance for checking that that the

typo

Suggestion:

coefficient_tol

---

common/symbolic/decompose.cc line 12 at r2 (raw file):

#include "drake/common/fmt_eigen.h"
#include "drake/math/matrix_util.h"

nit Unused matrix_util (likewise in BUILD).

---

common/symbolic/decompose.cc line 353 at r2 (raw file):

  if (psd_tol < 0) {
    throw std::runtime_error("psd_tol should be non-negative.");
  }

This if-clause fails to detect NaNs.

(That's why DRAKE_THROW_UNLESS uses positive phrasing instead of negative. If you want to keep this code as if-then-throw form, then the clause must be written in the not-success phrasing i.e. if (!(psd_tol >= 0)) { throw ... }.)

Suggestion:

DRAKE_THROW_UNLESS(psd_tol >= 0);

---

common/symbolic/decompose.cc line 354 at r2 (raw file):

    throw std::runtime_error("psd_tol should be non-negative.");
  }

Reading the docs, it seems to me like a negative coefficient_tol should also be an exception?

---

common/symbolic/decompose.cc line 385 at r2 (raw file):

  // math::DecomposePSDmatrixIntoXtransposeTimesX.
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(Q);
  DRAKE_THROW_UNLESS(eigensolver.info() == Eigen::Success);

Why do we throw instead of returning is_l2norm == false? Our API docs don't seem to allow us to throw on ill-conditioned costs.

If this will never fail no matter what the user gave us, then we want DRAKE_DEMAND here to communicate as much (along with some comment explaining why it can never fail).

(But see below as well, since this might anyway be moot.)

---

common/symbolic/decompose.cc line 389 at r2 (raw file):

    return {false, A, b, vars};
  }
  A = math::DecomposePSDmatrixIntoXtransposeTimesX(Q, psd_tol);

Hmm, didn't we already compute the SelfAdjointEigenSolver result just above, why are we calling this function that will recompute it again? I guess we computed it above to check for the psd_tol violation and return false instead of throwing, but if we're going to call DecomposePSDmatrixIntoXtransposeTimesX we shouldn't be re-implementing its failure checks as ahead-of-time guesses, we should change it to have a status-output option instead of throwing and let it tell us what worked or didn't. (There is a TODO inside of it to stop using SelfAdjoint so after that's fixed this code will be brittle / wrong.)

---

common/symbolic/test/decompose_test.cc line 518 at r2 (raw file):

  const auto [is_l2norm, A, b, vars] = DecomposeL2NormExpression(e);
  EXPECT_TRUE(is_l2norm);
  EXPECT_TRUE(CompareMatrices(A.transpose() * A,

The names A and A_expected beg the question of why we're not comparing those two matrices directly, rather than the more complicated A'A and A'b equivalence checks. For those who don't know the math off the top of their heads, we should probably have a comment here why we need to use the products here while comparing.

---

common/symbolic/test/decompose_test.cc line 581 at r2 (raw file):

  EXPECT_FALSE(is_l2norm);

  // Not a positive quadratic form insider the sqrt.

typo

Suggestion:

inside

---

solvers/create_cost.cc line 103 at r2 (raw file):

      DecomposeL2NormExpression(e, psd_tol, coefficient_tol);
  if (!is_l2norm) {
    throw runtime_error("Expression " + e.to_string() +

nit For readability, don't use string::operator+ for error messages, use fmt -- i.e., fmt::format("Expression {} is not ...", ...).

---

solvers/mathematical_program.cc line 540 at r2 (raw file):

Binding<L2NormCost> MathematicalProgram::AddL2NormCost(
    const symbolic::Expression& e, double psd_tol, double constant_term_tol) {

typo

Suggestion:

coefficient_tol

---

solvers/test/mathematical_program_test.cc line 3301 at r2 (raw file):

  EXPECT_EQ(prog.l2norm_costs().size(), 3u);

  auto obj4 = prog.AddCost(e);

nit The use of AddCost here (vs AddL2NormCost everywhere else in this function) is too subtle to happen silently. We need a comment here saying that we're testing the AddCost() function recognizing of an L2norm.

[jwnimmer-tri]

Reviewable status: 14 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform)

---

common/symbolic/BUILD.bazel line 187 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Working

This is a newly dependency cycle between common <=> math. I need to give it some consideration to see if we need some kind of surgery to avoid it.

(If there is a sensible way to rewrite the code to not have such a cycle, that would probably be better.)

Okay, this is not too terrible. Removing the dependency would be better, but we can survive with just marking it implementation-only by splitting up here the interface_deps versus deps. The former keeps :expression and the latter is what lists the new stuff.

[RussTedrake]

Sorry for the delay. PTAL.

Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform)

---

common/symbolic/BUILD.bazel line 187 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Okay, this is not too terrible. Removing the dependency would be better, but we can survive with just marking it implementation-only by splitting up here the interface_deps versus deps. The former keeps :expression and the latter is what lists the new stuff.

Done. Thanks.

---

common/symbolic/decompose.h line 172 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

nit, typo, duplicated "that".

Done.

---

common/symbolic/decompose.h line 172 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

typo

Done.

---

common/symbolic/decompose.cc line 12 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Unused matrix_util (likewise in BUILD).

Done.

---

common/symbolic/decompose.cc line 353 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This if-clause fails to detect NaNs.

(That's why DRAKE_THROW_UNLESS uses positive phrasing instead of negative. If you want to keep this code as if-then-throw form, then the clause must be written in the not-success phrasing i.e. if (!(psd_tol >= 0)) { throw ... }.)

Done.

---

common/symbolic/decompose.cc line 354 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Reading the docs, it seems to me like a negative coefficient_tol should also be an exception?

Done.

---

common/symbolic/decompose.cc line 371 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

nit, consider using std::move here to avoid copying.

Done.

---

common/symbolic/decompose.cc line 385 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Why do we throw instead of returning is_l2norm == false? Our API docs don't seem to allow us to throw on ill-conditioned costs.

If this will never fail no matter what the user gave us, then we want DRAKE_DEMAND here to communicate as much (along with some comment explaining why it can never fail).

(But see below as well, since this might anyway be moot.)

Done. (per below)

---

common/symbolic/decompose.cc line 389 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Hmm, didn't we already compute the SelfAdjointEigenSolver result just above, why are we calling this function that will recompute it again? I guess we computed it above to check for the psd_tol violation and return false instead of throwing, but if we're going to call DecomposePSDmatrixIntoXtransposeTimesX we shouldn't be re-implementing its failure checks as ahead-of-time guesses, we should change it to have a status-output option instead of throwing and let it tell us what worked or didn't. (There is a TODO inside of it to stop using SelfAdjoint so after that's fixed this code will be brittle / wrong.)

Done. I had to think about how to make that update without changing / deprecating the output type of DecomposePSDMatrixIntoXTransposeTimesX. But based on the math, I think returning an empty matrix is appropriate... given a sufficiently descriptive option name. @hongkai-dai ... wdyt?

---

common/symbolic/test/decompose_test.cc line 518 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The names A and A_expected beg the question of why we're not comparing those two matrices directly, rather than the more complicated A'A and A'b equivalence checks. For those who don't know the math off the top of their heads, we should probably have a comment here why we need to use the products here while comparing.

Done.

---

common/symbolic/test/decompose_test.cc line 581 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

typo

done.

---

solvers/create_cost.cc line 103 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit For readability, don't use string::operator+ for error messages, use fmt -- i.e., fmt::format("Expression {} is not ...", ...).

Done.

---

solvers/mathematical_program.cc line 540 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

typo

Done.

---

solvers/test/mathematical_program_test.cc line 3301 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit The use of AddCost here (vs AddL2NormCost everywhere else in this function) is too subtle to happen silently. We need a comment here saying that we're testing the AddCost() function recognizing of an L2norm.

Done.

[jwnimmer-tri]

but it sounds like given the ping we should wait for @hongkai-dai to weigh in on the new option.

Reviewed 12 of 12 files at r3, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),hongkai-dai

[hongkai-dai]

Reviewed 12 of 12 files at r3, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees jwnimmer-tri(platform),hongkai-dai

---

common/symbolic/decompose.cc line 389 at r2 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Done. I had to think about how to make that update without changing / deprecating the output type of DecomposePSDMatrixIntoXTransposeTimesX. But based on the math, I think returning an empty matrix is appropriate... given a sufficiently descriptive option name. @hongkai-dai ... wdyt?

Returning an empty matrix works for me.