Add equality constraints in LQR.

[hongkai-dai]

This change is 

[hongkai-dai]

+@RussTedrake for feature review please, thanks!

Reviewable status: LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @RussTedrake)

[RussTedrake]

Sorry for being slow. I've found a few small issues. PTAL.

Reviewed 5 of 5 files at r1, all commit messages.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @hongkai-dai)

---

systems/controllers/linear_quadratic_regulator.h line 20 at r1 (raw file):

///
///   @f[ \dot{x} = Ax + Bu @f]
///   @f[ \min_u \int_0^\infty x'Qx + u'Ru + 2x'Nu dt @f]

nit: could we move the dynamic to underneath the objective, so that it's in the standard minimize _ subject to __ form?

---

systems/controllers/linear_quadratic_regulator.h line 32 at r1 (raw file):

/// zero-sized, N will be treated as a num_states x num_inputs zero matrix.
/// @param F A constraint matrix of size num_constraints x num_states. rank(F)
/// must be < num_states. If the matrix is zero-sized, F will be treated as a 0

"if the matrix is zero-sized". that's not quite the condition you check; because you check if rows != 0, then cols must == N. if the user passes an 4 x 0 matrix, that would still be zero-sized, but will fail the DRAKE_DEMAND.

---

systems/controllers/linear_quadratic_regulator.cc line 44 at r1 (raw file):

  if (R_cholesky.info() != Eigen::Success)
    throw std::runtime_error("R must be positive definite");
  // The rows of P are the basis for the null-space of F, namely PPᵀ = I and

nit: orthonormal basis.

---

systems/controllers/linear_quadratic_regulator.cc line 49 at r1 (raw file):

  if (F.rows() != 0) {
    // Compute the null-space based on QR decomposition. If Fᵀ*S = [Q1 Q2][R; 0]
    // = Q1*R, then take P=Q2ᵀ, where S is the permutation matrix.

yikes. Q, R, and S already have different meanings in this function, making this documentation pretty confusing.

---

systems/controllers/linear_quadratic_regulator.cc line 52 at r1 (raw file):

    // We introduce projected state y, such that y = Px.
    // We form a new dynamical system
    // ẏ = PAPᵀx + PBu

the x should be y.

---

systems/controllers/linear_quadratic_regulator.cc line 88 at r1 (raw file):

      //   = -R⁻¹(BᵀS+NᵀPᵀP)
      ret.K = R_cholesky.solve(B.transpose() * ret.S +
                               N.transpose() * P->transpose() * (*P));

P^T * P is the identity; you shouldn't need that here?

[hongkai-dai]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @RussTedrake)

---

systems/controllers/linear_quadratic_regulator.h line 32 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

"if the matrix is zero-sized". that's not quite the condition you check; because you check if rows != 0, then cols must == N. if the user passes an 4 x 0 matrix, that would still be zero-sized, but will fail the DRAKE_DEMAND.

Done. I also changed the documentation for N above. We also checked DRAKE_DEMAND(N.cols() == m).

---

systems/controllers/linear_quadratic_regulator.cc line 49 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

yikes. Q, R, and S already have different meanings in this function, making this documentation pretty confusing.

Done. Good call. I factored out the function to compute the kernel.

---

systems/controllers/linear_quadratic_regulator.cc line 52 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

the x should be y.

Done.

---

systems/controllers/linear_quadratic_regulator.cc line 88 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

P^T * P is the identity; you shouldn't need that here?

I don't think P^T*P is identity, we have P * P^T being identity, but swapping the order of P and P^T won't give us identity matrix, unless they are both full-rank square matrices.

One example is P = [1/sqrt(2), 1/sqrt(2)], we can see that P * P^T = 1, but P^T * P = [0.5, 0.5; 0.5 0.5]

[RussTedrake]

Reviewed 2 of 2 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion, needs at least two assigned reviewers, missing label for release notes (waiting on @hongkai-dai)

---

systems/controllers/linear_quadratic_regulator.cc line 88 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I don't think P^T*P is identity, we have P * P^T being identity, but swapping the order of P and P^T won't give us identity matrix, unless they are both full-rank square matrices.

One example is P = [1/sqrt(2), 1/sqrt(2)], we can see that P * P^T = 1, but P^T * P = [0.5, 0.5; 0.5 0.5]

My bad! Retracted.

[hongkai-dai]

+@EricCousineau-TRI for platform review please, thanks!

Reviewable status: 1 unresolved discussion, LGTM missing from assignee EricCousineau-TRI(platform), missing label for release notes (waiting on @EricCousineau-TRI and @RussTedrake)

---

systems/controllers/linear_quadratic_regulator.cc line 88 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

My bad! Retracted.

Done.

[EricCousineau-TRI]

plaform, with very small nits / BTWs. Nice!

Reviewed 3 of 5 files at r1, 2 of 2 files at r2, all commit messages.
Reviewable status: 3 unresolved discussions, missing label for release notes (waiting on @hongkai-dai and @RussTedrake)

---

systems/controllers/linear_quadratic_regulator.h line 21 at r2 (raw file):

///   @f[ \min_u \int_0^\infty x'Qx + u'Ru + 2x'Nu dt @f]
///   @f[ \dot{x} = Ax + Bu @f]
///   @f[ Fx=0 @f]

nit Above code has a space between =. Consider using that here.

---

systems/controllers/linear_quadratic_regulator.h line 32 at r2 (raw file):

/// 0, N will be treated as a num_states x num_inputs zero matrix.
/// @param F A constraint matrix of size num_constraints x num_states. rank(F)
/// must be < num_states. If the F.rows() == 0, F will be treated as a 0

BTW the F.rows() sounds a bit odd. Consider removing the

---

systems/controllers/test/linear_quadratic_regulator_test.cc line 18 at r2 (raw file):

namespace {

GTEST_TEST(TestLQR, TestException) {

BTW (pre-existing) For acronyms, style guide says to prefer TestLqr over TestLQR
https://drake.mit.edu/styleguide/cppguide.html#General_Naming_Rules

---

systems/controllers/test/linear_quadratic_regulator_test.cc line 124 at r2 (raw file):

// Test if the LQR solution satisfies the HJB equality
// minᵤ xᵀQx + uᵀRu + 2xᵀNu + 2xᵀS(Ax+Bu) = 0
void TestLQRwHJB(

nit Not the most readable name.

Consider expanding to TestLQRwithHJB, or following style guide for acroynyms, TestLqrWithHjb

---

systems/controllers/test/linear_quadratic_regulator_test.cc line 134 at r2 (raw file):

  const int nx = A.rows();
  const int nu = B.cols();
  // We first try to remove the constraint F*x=0 by considering a new slack

BTW Missing space around = (for consistency)

---

systems/controllers/test/linear_quadratic_regulator_test.cc line 149 at r2 (raw file):

    ASSERT_EQ(qr_F.info(), Eigen::Success);
    const Eigen::MatrixXd F_Q = qr_F.matrixQ();
    P = F_Q.rightCols(nx - qr_F.rank()).transpose();

BTW Consider exposing ComputerKernel in internal, which you could then re-use here.

[hongkai-dai]

Reviewable status: 1 unresolved discussion (waiting on @RussTedrake)

---

systems/controllers/linear_quadratic_regulator.cc line 88 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Done.

I added some documentation to explain it here.

---

systems/controllers/test/linear_quadratic_regulator_test.cc line 18 at r2 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

BTW (pre-existing) For acronyms, style guide says to prefer TestLqr over TestLQR
https://drake.mit.edu/styleguide/cppguide.html#General_Naming_Rules

Done.

---

systems/controllers/test/linear_quadratic_regulator_test.cc line 124 at r2 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

nit Not the most readable name.

Consider expanding to TestLQRwithHJB, or following style guide for acroynyms, TestLqrWithHjb

DOne.

---

systems/controllers/test/linear_quadratic_regulator_test.cc line 149 at r2 (raw file):

Previously, EricCousineau-TRI (Eric Cousineau) wrote…

BTW Consider exposing ComputerKernel in internal, which you could then re-use here.

For the moment I am hesitant to expose this function. The reason is that there are many approaches to compute the kernels, and here I use QR decomposition with column pivoting. One can also try other rank-revealing decomposition to compute the kernel. I put a TODO here to expose this function in the future. Would this be good?

[EricCousineau-TRI]

Reviewed 3 of 3 files at r3, all commit messages.
Dismissed @RussTedrake from a discussion.
Reviewable status: complete! all discussions resolved, LGTM from assignees EricCousineau-TRI(platform),RussTedrake(platform) (waiting on @hongkai-dai)

---

systems/controllers/test/linear_quadratic_regulator_test.cc line 149 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

For the moment I am hesitant to expose this function. The reason is that there are many approaches to compute the kernels, and here I use QR decomposition with column pivoting. One can also try other rank-revealing decomposition to compute the kernel. I put a TODO here to expose this function in the future. Would this be good?

OK Yup, works for me!