Convert kintrajopt to use AsLinearInControlPoints

[RussTedrake]

Resolves #22533.

This includes a small breaking change. The vector<vector> return types for KinematicTrajectoryOptimization AddAccelerationBounds and AddJerkBounds are now vector.

+@cohnt for feature review, please? This cleaning things up significantly, imo.

---

This change is 

[cohnt]

A few nits, but overall

Reviewed 3 of 3 files at r1, all commit messages.
Reviewable status: 7 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers

---

planning/trajectory_optimization/test/kinematic_trajectory_optimization_test.cc line 255 at r1 (raw file):

                                            VectorXd::Ones(num_positions_));
  EXPECT_THAT(binding[0].to_string(), HasSubstr("velocity lower bound"));
  EXPECT_THAT(binding[1].to_string(), HasSubstr("velocity upper bound"));

btw After seeing the change to the docstring that specifies the order that the bindings are interleaved, I immediately scrolled to the test cases to make sure it was being tested. Happy to see you had already thought of that 😄

---

planning/trajectory_optimization/kinematic_trajectory_optimization.h line 183 at r1 (raw file):

  respected at all times.  Note that this does NOT directly constrain r̈(s).

  @returns A vector of bindings constraining all of the control points for one

nit I found the @returns description unclear. What is "position"? Could be position in the vector, position in space, maybe something else? Also, I don't quite see what the interleaving order is just from the documentation.

I think part of why this confused me is that this is not a pointwise constraint, but rather applied at all times. From a closer read, I can see that it's applying the constraint to each control point of the acceleration trajectory, which guarantees constraint satisfaction at all times by the convex hull property. Maybe mentioning the convex hull property would help? (I.e. explaining why this constraint can be enforced everywhere, whereas others can't?)

---

planning/trajectory_optimization/kinematic_trajectory_optimization.h line 194 at r1 (raw file):

  d³rds³(s).

  @returns A vector of bindings constraining all of the control points for one

nit ditto my comment above re documentation.

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 96 at r1 (raw file):

  duration = x[0]
  control_points = x[1:num_control_points].reshaped(num_positions,
  num_control_points) q = control_points * a_pos v = control_points * a_vel

nit looks like the linter screwed up how this is supposed to be written?

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 190 at r1 (raw file):

// For MatrixXDecisionVariable X and VectorXd a, returns SparseMatrix A and
// VectorXDecisionVariable x such that Ax == Xa.

nit I found this a little confusing, because it was pretty unclear whether or not this is creating a constraint. It didn't really make sense until I saw how it was being used, and then came back to reread the function. I think maybe specifying that the entries of the vector x are actually the same variables as those in the matrix X would help?

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 452 at r1 (raw file):

  // These are nonconvex constraints, but they again leverage the convex hull
  // property to enforce the guarantee ∀t by only constraining the values at
  // the control points.

nit can you list out how the constraints are written, like what you did above for AddVelocityBounds? E.g.

// lb(i)*duration <= derivative_control_points(i,:) * M, but transposed

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 481 at r1 (raw file):

  SparseMatrix<double> A = M.transpose();
  VectorXDecisionVariable vars(1 + num_control_points_);
  vars[0] = duration_;

nit ditto above re explaining the constraints

[RussTedrake]

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers

---

planning/trajectory_optimization/kinematic_trajectory_optimization.h line 183 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit I found the @returns description unclear. What is "position"? Could be position in the vector, position in space, maybe something else? Also, I don't quite see what the interleaving order is just from the documentation.

I think part of why this confused me is that this is not a pointwise constraint, but rather applied at all times. From a closer read, I can see that it's applying the constraint to each control point of the acceleration trajectory, which guarantees constraint satisfaction at all times by the convex hull property. Maybe mentioning the convex hull property would help? (I.e. explaining why this constraint can be enforced everywhere, whereas others can't?)

Done.

---

planning/trajectory_optimization/kinematic_trajectory_optimization.h line 194 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit ditto my comment above re documentation.

Done.

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 96 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit looks like the linter screwed up how this is supposed to be written?

Done.

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 190 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit I found this a little confusing, because it was pretty unclear whether or not this is creating a constraint. It didn't really make sense until I saw how it was being used, and then came back to reread the function. I think maybe specifying that the entries of the vector x are actually the same variables as those in the matrix X would help?

Done.

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 452 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit can you list out how the constraints are written, like what you did above for AddVelocityBounds? E.g.

// lb(i)*duration <= derivative_control_points(i,:) * M, but transposed

Done.

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 481 at r1 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit ditto above re explaining the constraints

Done.

[cohnt]

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

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 194 at r2 (raw file):

// variables on the left), it is still a vector constraint, but we must do some
// work to rewrite it as A*x, and the result is a big sparse A. This method
// does precisely that.For MatrixXDecisionVariable X and VectorXd a, returns

nit Need a space after the period.

[RussTedrake]

+@xuchenhan-tri for platform review, please.

Reviewable status: LGTM missing from assignee xuchenhan-tri(platform)

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 194 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit Need a space after the period.

Done.

[cohnt]

Reviewed 1 of 1 files at r3, all commit messages.
Reviewable status: LGTM missing from assignee xuchenhan-tri(platform)

[xuchenhan-tri]

Platform with a few comments.

Reviewed 1 of 3 files at r1, 1 of 2 files at r2, 1 of 1 files at r3, all commit messages.
Reviewable status: 4 unresolved discussions

---

a discussion (no related file):
I feel obliged to ask this: have you considered a non-breaking way to add this change?

The semantic of the function seems to be overhauled. Have you considered introducing a new name for the function?

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 101 at r3 (raw file):

class WrappedVelocityConstraint : public Constraint {
 public:
  WrappedVelocityConstraint(std::shared_ptr<Constraint> wrapped_constraint,

nit, DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN for all these constraint subclasses?

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 369 at r3 (raw file):

      bspline_.EvaluateLinearInControlPoints(s, /* derivative_order= */ 2);
  // lb <= control_points_ * M <= ub
  auto [A, x] = RewriteXa(control_points_, M);

nit const?

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 427 at r3 (raw file):

  vars[0] = duration_;
  SparseMatrix<double> A(M.cols(), M.rows() + 1);
  A.rightCols(M.rows()) = M.transpose();

BTW, TIL you could do block operations on SparseMatrix. Do you know what the memory implication of this is?

[RussTedrake]

Reviewable status: 1 unresolved discussion

---

a discussion (no related file):

Previously, xuchenhan-tri wrote…

I feel obliged to ask this: have you considered a non-breaking way to add this change?

The semantic of the function seems to be overhauled. Have you considered introducing a new name for the function?

It's a fair question. The practice of returning the added constraints has been considered good practice, but i think these return values are almost never used in any but trivial ways (e.g. perhaps deleting the constraints that were added). But in these cases, I feel that it is exposing too much of the internal implementation details, which ideally should be allowed to change without being considered breaking.

I think the clarity of the nuanced method names in the KinematicTrajectoryOptimization class is essential. This is a case where I don't want to obfuscate the API to dance around a change that will almost certainly impact almost nobody (but i agree it is a part of our current contract).

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 101 at r3 (raw file):

Previously, xuchenhan-tri wrote…

nit, DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN for all these constraint subclasses?

Done.

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 369 at r3 (raw file):

Previously, xuchenhan-tri wrote…

nit const?

Done.

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 427 at r3 (raw file):

Previously, xuchenhan-tri wrote…

BTW, TIL you could do block operations on SparseMatrix. Do you know what the memory implication of this is?

Only columnwise block operations are permitted on columnwise sparse matrices. The api tries to prevent you from doing bad things. What memory implications are you worried about?

[RussTedrake]

Reviewable status: 1 unresolved discussion

---

a discussion (no related file):

Previously, RussTedrake (Russ Tedrake) wrote…

It's a fair question. The practice of returning the added constraints has been considered good practice, but i think these return values are almost never used in any but trivial ways (e.g. perhaps deleting the constraints that were added). But in these cases, I feel that it is exposing too much of the internal implementation details, which ideally should be allowed to change without being considered breaking.

I think the clarity of the nuanced method names in the KinematicTrajectoryOptimization class is essential. This is a case where I don't want to obfuscate the API to dance around a change that will almost certainly impact almost nobody (but i agree it is a part of our current contract).

I had a chat with Jeremy about this this morning. I've added a note to the return type doxygen to make it clear for the future that the specifics of the return constraints may change without deprecation. (in this case, we're also changing the type, so that documentation doesn't full cover the break here even in the future... but I think std:vector is the right generic form that we can hide behind forevermore; it was a mistake to return something different before.

Jeremy didn't weigh in on deprecating or not deprecating (although he suggested adding a 2 to the end of the function name is common practice and wouldn't be offensive to him).

My preference is to rip the bandaid off this time and try to be more careful with documenting the return value contract going forward.

[xuchenhan-tri]

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

---

planning/trajectory_optimization/kinematic_trajectory_optimization.cc line 427 at r3 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Only columnwise block operations are permitted on columnwise sparse matrices. The api tries to prevent you from doing bad things. What memory implications are you worried about?

Ok, I was confused before because I didn't find public documentation on what operating on (especially writing into) blocks of a sparse matrix.

Turned out this is documented internally only (https://eigen.tuxfamily.org/dox/SparseBlock_8h_source.html). That answered my question.