Implement Bspline "linear in control points" logic

[RussTedrake]

For BsplineBasis and BsplineTrajectory.

Towards #22533 which is towards #22500.

+@cohnt for feature review, please?

---

This change is 

[RussTedrake]

bindings/pydrake/test/trajectories_test.py line 241 at r1 (raw file):

        M = bspline.EvaluateLinearInControlPoints(
            parameter_value=1.5, derivative_order=1)
        self.assertEqual(M.shape, (2,))

note: in my local testing, I'm getting a failure here if both of these bspline calls are included (commenting out either one allows the test to pass). I've verified that the code is fine if run in c++. But I can't see why the bindings should be anything more than the trivial ones. @rpoyner-tri -- might you have any ideas?

[cohnt]

Some small nits, but one real question about the behavior when the derivative order is higher than the basis order.

Reviewed 11 of 11 files at r1, all commit messages.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned, needs at least two assigned reviewers

---

common/trajectories/bspline_trajectory.h line 78 at r1 (raw file):

  /** Returns the vector, M, such that
  @verbatim
  EvalDerivative(t, derivative_order) = control_points() * M

nit: this.control_points() (so it matches the stub above)

---

math/bspline_basis.cc line 99 at r1 (raw file):

  DRAKE_THROW_UNLESS(parameter_value >= initial_parameter_value());
  DRAKE_THROW_UNLESS(parameter_value <= final_parameter_value());

btw same comment above re the math. It looks right to me, but I don't have much experience with Bsplines.

---

common/trajectories/bspline_trajectory.cc line 41 at r1 (raw file):

    return M;
  } else if (derivative_order >= basis_.order()) {
    // In this case, MakeDerivative will return a zero trajectory using a

I found this behavior a bit unintuitive. After thinking about it, it does make sense, but this seems like it's creating a foot gun for future users, where they think they're constructing some linear expression of the control points, but actually, they've just made a vector of zeros (and therefore, a bunch of constraints of the form 1=0.)

At the bare minimum, I think we should explicitly document what's happening. E.g., add If derivative_order >= basis_.order(), then this derivative is just zero, so the matrix returned is the zero matrix. But I'd even suggest throwing in this instance, since I can't think of any use case where we would care about getting that matrix out, and it's almost certainly a mistake.

---

common/trajectories/bspline_trajectory.cc line 51 at r1 (raw file):

    for (int j = 1; j <= derivative_order; ++j) {
      for (int i = 0; i < num_control_points() - j; ++i) {
        // pᵢᵏ⁺¹ = αᵢ * (pᵢ₊₁ᵏ - pᵢᵏ), and pᵢᵏ = p * Mᵢᵏ, where the i subscript

This looks right to me, but I'm not super familiar with the math behind bsplines.

---

common/trajectories/bspline_trajectory.cc line 98 at r1 (raw file):

            (basis_.knots()[i + basis_.order()] - basis_.knots()[i + j]) *
            (M_k.col(i + 1) - M_k.col(i));
      }

nit This little snippet here is more-or-less duplicated several times throughout the file now. In AsLinearInControlPoints, EvaluateLinearInControlPoints, DoEvalDerivative, and DoMakeDerivative. As part of this PR, what do you think of breaking that off into its own method? (Or alternatively, making some of these methods call the other methods?)

[rpoyner-tri]

Reviewable status: 6 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned, needs at least two assigned reviewers

---

bindings/pydrake/test/trajectories_test.py line 241 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

note: in my local testing, I'm getting a failure here if both of these bspline calls are included (commenting out either one allows the test to pass). I've verified that the code is fine if run in c++. But I can't see why the bindings should be anything more than the trivial ones. @rpoyner-tri -- might you have any ideas?

👀 maybe will get to this later today.

[rpoyner-tri]

Reviewable status: 6 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned, needs at least two assigned reviewers

---

bindings/pydrake/test/trajectories_test.py line 241 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

👀 maybe will get to this later today.

I've reproduced the symptoms, but first look at the c++ and the bindings doesn't suggest the problem. I'll keep looking.

[rpoyner-tri]

bindings/pydrake/test/trajectories_test.py line 241 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

I've reproduced the symptoms, but first look at the c++ and the bindings doesn't suggest the problem. I'll keep looking.

It appears that T=float (double, really) doesn't crash, but both autodiff and symbolic do crash.

Elsewhere (plant_test.py), I've found this comment:

Code snippet:

# Bindings for Eigen::SparseMatrix only support T=float for now.

[rpoyner-tri]

Reviewable status: 6 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned, needs at least two assigned reviewers

---

bindings/pydrake/test/trajectories_test.py line 241 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

It appears that T=float (double, really) doesn't crash, but both autodiff and symbolic do crash.

Elsewhere (plant_test.py), I've found this comment:

Not sure what the history of all that is yet, but it appears this might be the only existing attempt to bind SparseMatrix for non-double.

[rpoyner-tri]

Reviewable status: 6 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned, needs at least two assigned reviewers

---

bindings/pydrake/test/trajectories_test.py line 241 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Not sure what the history of all that is yet, but it appears this might be the only existing attempt to bind SparseMatrix for non-double.

Ah, you've been here before: #19712

[RussTedrake]

Thanks Tommy. And I've fixed the binding error thanks to rico. PTAL.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned, needs at least two assigned reviewers

---

common/trajectories/bspline_trajectory.h line 78 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit: this.control_points() (so it matches the stub above)

But it's different. The previous one has two different trajectory objects, this one does not. if I added this here, I'd need it on the EvalDerivative, too... which starts to get silly.

---

common/trajectories/bspline_trajectory.cc line 41 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

I found this behavior a bit unintuitive. After thinking about it, it does make sense, but this seems like it's creating a foot gun for future users, where they think they're constructing some linear expression of the control points, but actually, they've just made a vector of zeros (and therefore, a bunch of constraints of the form 1=0.)

At the bare minimum, I think we should explicitly document what's happening. E.g., add If derivative_order >= basis_.order(), then this derivative is just zero, so the matrix returned is the zero matrix. But I'd even suggest throwing in this instance, since I can't think of any use case where we would care about getting that matrix out, and it's almost certainly a mistake.

Done. I've added a note to the docstring.

But I definitely disagree about throwing. For me, the footgun would be if you take successive derivatives, and suddenly explode. The derivative is perfectly well defined here. Setting e.g. jerk limits \in [-limit, limit] would work just fine, for instance, even if the order was too low to ever have non-zero jerk limits.

---

common/trajectories/bspline_trajectory.cc line 98 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit This little snippet here is more-or-less duplicated several times throughout the file now. In AsLinearInControlPoints, EvaluateLinearInControlPoints, DoEvalDerivative, and DoMakeDerivative. As part of this PR, what do you think of breaking that off into its own method? (Or alternatively, making some of these methods call the other methods?)

Done. (?) It's a good point. I actually cleaned things up (and actually made one of the cases more efficient). But each of them is different in slightly important ways... they are different types (matrix vs std::vector) and have different ranges (all coefficients or just the active coefficients). To the point that my attempt to unify felt uglier than this. And the fact that the derivatives took algebraically the same form as the original was non-obvious... i don't even know how I'd make the arguments clear on that point? So I've tidied, but not actually extracted that bit.

---

bindings/pydrake/test/trajectories_test.py line 241 at r1 (raw file):

Previously, rpoyner-tri (Rick Poyner (rico)) wrote…

Ah, you've been here before: #19712

Done. amazing. thanks for diagnosing that, and sorry that I didn't remember my own history with this... 🤦‍♂️ I've applied the same fix here.

[cohnt]

Reviewed 4 of 4 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers

---

common/trajectories/bspline_trajectory.h line 78 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

But it's different. The previous one has two different trajectory objects, this one does not. if I added this here, I'd need it on the EvalDerivative, too... which starts to get silly.

Ah, I see the distinction. Sounds good then.

---

common/trajectories/bspline_trajectory.cc line 41 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Done. I've added a note to the docstring.

But I definitely disagree about throwing. For me, the footgun would be if you take successive derivatives, and suddenly explode. The derivative is perfectly well defined here. Setting e.g. jerk limits \in [-limit, limit] would work just fine, for instance, even if the order was too low to ever have non-zero jerk limits.

I think adding a note in the docstring is sufficient.

---

common/trajectories/bspline_trajectory.cc line 98 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Done. (?) It's a good point. I actually cleaned things up (and actually made one of the cases more efficient). But each of them is different in slightly important ways... they are different types (matrix vs std::vector) and have different ranges (all coefficients or just the active coefficients). To the point that my attempt to unify felt uglier than this. And the fact that the derivatives took algebraically the same form as the original was non-obvious... i don't even know how I'd make the arguments clear on that point? So I've tidied, but not actually extracted that bit.

If unifying didn't end up being a clean solution, then what you have now is fine. (I didn't notice the typing differences, so I can definitely see that making unification too complicated to be worth it.)

The cleanup to DoMakeDerivative is certainly a lot nicer.

---

bindings/pydrake/test/trajectories_test.py line 209 at r2 (raw file):

        numpy_compare.assert_float_equal(curve.control_points(), points)

        if T == float:

nit Maybe tag these with #19712 as well? In case someone is reading the python test files but not the C++ file where the bindings are created.

[RussTedrake]

+@rpoyner-tri for platform review, please.

Reviewable status: 1 unresolved discussion, LGTM missing from assignee rpoyner-tri(platform)

[rpoyner-tri]

Reviewed 7 of 11 files at r1, 4 of 4 files at r2, all commit messages.
Reviewable status: 1 unresolved discussion

[RussTedrake]

Reviewable status: complete! all discussions resolved, LGTM from assignees rpoyner-tri(platform),cohnt

---

bindings/pydrake/test/trajectories_test.py line 209 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit Maybe tag these with #19712 as well? In case someone is reading the python test files but not the C++ file where the bindings are created.

Done.

[rpoyner-tri]

Reviewed 1 of 1 files at r3, all commit messages.
Reviewable status: complete! all discussions resolved, LGTM from assignees rpoyner-tri(platform),cohnt