trajectories: Switch to Sparse solver for constructing CubicWithContinuousSecondDerivatives

[mpetersen94]

Also, switches from using 4 constraints per knot point to only 3 constraints per knot point by removing the trivial constraint that the constant term is equal to the knot point location.

---

This change is 

[mpetersen94]

@RussTedrake or @siyuanfeng-tri for feature review.

[SeanCurtis-TRI]

+@siyuanfeng-tri

(I'm explicitly assigning this to @siyuanfeng-tri and he can pass it along if this assignment isn't appropriate.)

Reviewable status: LGTM missing from assignee siyuanfeng-tri, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @siyuanfeng-tri)

[mpetersen94]

@drake-jenkins-bot linux-bionic-unprovisioned-gcc-bazel-experimental-snopt-packaging please

[siyuanfeng-tri]

thanks! minor comments. i am curious if you have done a speed check for the sparse solver?

Reviewed 2 of 2 files at r1.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee siyuanfeng-tri, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mpetersen94)

---

common/trajectories/piecewise_polynomial.cc, line 827 at r1 (raw file):

  int row_idx = 0;
  std::vector<Eigen::Triplet<T>>& triplet_ref = *triplet_list;
  VectorX<T>& bref = *b;

nit, should we set bref[0] = 0 to be extra safe?

---

common/trajectories/piecewise_polynomial.cc, line 908 at r1 (raw file):

    for (int k = 0; k < cols; ++k) {
      std::vector<Eigen::Triplet<T>> triplet_list;
      triplet_list.reserve(10 * (N - 2) + 7);

can you add a comment to how this capacity is computed? thanks!

[siyuanfeng-tri]

also i am in a shanghai timezone, so i won't make comments blocking, feel free to move to platform.

[mpetersen94]

I did do a rough speed check and for a PiecewisePolynomial generated from 100 knot points (each of size 10), the sparse solver was 100 times faster.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee siyuanfeng-tri, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @siyuanfeng-tri)

---

common/trajectories/piecewise_polynomial.cc, line 827 at r1 (raw file):

Previously, siyuanfeng-tri wrote…

nit, should we set bref[0] = 0 to be extra safe?

I don't understand what that would do. Is the concern what the behavior is if N-1 == 0?

---

common/trajectories/piecewise_polynomial.cc, line 908 at r1 (raw file):

Previously, siyuanfeng-tri wrote…

can you add a comment to how this capacity is computed? thanks!

Done.

[mpetersen94]

@SeanCurtis-TRI for platform review

[SeanCurtis-TRI]

I'm relying on feature review and existing unit tests to feel completely confident. Otherwise:

(With one note about documentation.)

Reviewed 2 of 2 files at r3.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee siyuanfeng-tri, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mpetersen94 and @siyuanfeng-tri)

---

common/trajectories/piecewise_polynomial.h, line 825 at r3 (raw file):

      const std::vector<T>& breaks,
      const std::vector<MatrixX<T>>& samples, int row, int col,
      std::vector<Eigen::Triplet<T>>* triplet_list, VectorX<T>* b);

BTW I find it highly disturbing that the signature can change this much and yet, the documentation doesn't change at all. We've change from A to triplet_list and changed the semantics of b -- neither are documented.

Part of that is that A and b were never documented prior to this change. It would be kind of nice if this were shored up and the semantics of these parameters were included in the documentation.

However, as this is not a defect introduced by this PR, you're not obliged to correct it.

[mpetersen94]

When this is ready to merge, it would be nice if we do not squash to maintain the sequence of changes in the commit history. The commits are properly curated with that goal in mind.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee siyuanfeng-tri, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @SeanCurtis-TRI and @siyuanfeng-tri)

---

common/trajectories/piecewise_polynomial.h, line 825 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I find it highly disturbing that the signature can change this much and yet, the documentation doesn't change at all. We've change from A to triplet_list and changed the semantics of b -- neither are documented.

Part of that is that A and b were never documented prior to this change. It would be kind of nice if this were shored up and the semantics of these parameters were included in the documentation.

However, as this is not a defect introduced by this PR, you're not obliged to correct it.

I took a stab at the documentation. PTAL.

[SeanCurtis-TRI]

I've tested the commits, they seem well curated. We'll merge accordingly.

@siyuanfeng-tri, we're missing the lgtm from you.

Reviewed 1 of 1 files at r4.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee siyuanfeng-tri, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @siyuanfeng-tri)

[SeanCurtis-TRI]

+(status: do not merge) (we're going to merge with curated commits. We don't want anyone accidentally merging until we're ready.)

Reviewable status: 1 unresolved discussion, LGTM missing from assignee siyuanfeng-tri, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @siyuanfeng-tri)

[siyuanfeng-tri]

Reviewed 1 of 1 files at r4.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mpetersen94)

---

common/trajectories/piecewise_polynomial.cc, line 827 at r1 (raw file):

Previously, mpetersen94 (Mark Petersen) wrote…

I don't understand what that would do. Is the concern what the behavior is if N-1 == 0?

i just wanted to make sure we don't have uninitialized memory, and i think we are fine here after looking at it more.

[siyuanfeng-tri]

Reviewed 1 of 2 files at r3.
Reviewable status: labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @mpetersen94)

[mpetersen94]

@SeanCurtis-TRI Can you add the right label and merge this now?

[SeanCurtis-TRI]

-(status: do not merge) +(status: commits are properly curated)

Reviewable status: labeled "do not merge" (waiting on @mpetersen94)