KinematicTrajectoryOptimization adds a linear constraint on velocity.

[hongkai-dai]

This imposes a linear constraint on our decision variables.

This is inspired by the stackoverflow question https://stackoverflow.com/questions/79317170/question-about-kinematictrajectoryoptimization-addpathvelocityconstraint

---

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 @hongkai-dai)

[RussTedrake]

Checkpoint. Maybe best to answer the API questions first, in case it changes the implementation?

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

---

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

  auto [vars_vel, _] = symbolic::ExtractVariablesFromExpression(rdot);
  Eigen::MatrixXd M_vel(num_positions(), vars_vel.size());
  symbolic::DecomposeLinearExpressions(rdot, vars_vel, &M_vel);

btw -- for bezier curve, we got rid of the symbolic (see AsLinearInControlPoints), but I guess we don't have it for bsplinebasis yet. :-(

---

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

  /** Adds a linear constraint on trajectory velocity `q̇(t)`, evaluated at
  `s`. The constraint will be evaluated as if it is bound with variables
  corresponding to `q̇(T*s)` (as opposed to [q(T*s), q̇(T*s)] in

oh. that seems very confusing, no? I would think that the bindings should still be [q, qdot] here? there is no real cost to having some of the elements of the constraint be zero?

---

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

  solvers::Binding<solvers::LinearConstraint>
  AddVelocityLinearConstraintAtNormalizedTime(
      const std::shared_ptr<solvers::LinearConstraint>& constraint, double s);

Wouldn't it be more clear/helpful to just take A and lb/ub?

[hongkai-dai]

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

---

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

Previously, RussTedrake (Russ Tedrake) wrote…

oh. that seems very confusing, no? I would think that the bindings should still be [q, qdot] here? there is no real cost to having some of the elements of the constraint be zero?

The issue of associating the constraint with q is that the constraint becomes nonlinear.

If we were to associate the constraint with [q, qdot], then the constraint becomes

lb <= A * [q; qdot] <= ub

since q = r, qdot = rdot / T, we have

lb <= A * [r, rdot/T] <= ub

rearranging the equation we get

A * [r * T, rdot] - lb * T >= 0
A * [r * T, rodt] - ub * T <= 0

Notice that r and T multiplies together, making the constraint nonlinear.

Maybe I should rename the function? What about AddVelocityLinearConstraint?

---

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

Previously, RussTedrake (Russ Tedrake) wrote…

Wouldn't it be more clear/helpful to just take A and lb/ub?

My rationale was that if we want to specify lb <= qdot <= ub, then it is easier to pass a BoundingBoxConstraint (which only requires specifying lb and ub, but not A), than passing A, lb, ub. Does that make sense?

---

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

Previously, RussTedrake (Russ Tedrake) wrote…

btw -- for bezier curve, we got rid of the symbolic (see AsLinearInControlPoints), but I guess we don't have it for bsplinebasis yet. :-(

Done. I added a TODO here.

[RussTedrake]

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

---

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

Previously, hongkai-dai (Hongkai Dai) wrote…

The issue of associating the constraint with q is that the constraint becomes nonlinear.

If we were to associate the constraint with [q, qdot], then the constraint becomes

lb <= A * [q; qdot] <= ub

since q = r, qdot = rdot / T, we have

lb <= A * [r, rdot/T] <= ub

rearranging the equation we get

A * [r * T, rdot] - lb * T >= 0
A * [r * T, rodt] - ub * T <= 0

Notice that r and T multiplies together, making the constraint nonlinear.

Maybe I should rename the function? What about AddVelocityLinearConstraint?

I see. Thanks for the clear explanation, and sorry I was slow!

I do think that the shortest path forward would be to find yet another name to distinguish this. But as I look at the overall API right now, what I actually think would be better is if we make some placeholder variables for q and qdot, etc, and then allowed people to pass bindings instead of constraints to this method. Then they could keep the same name AddVelocityConstraintAtNormalizedTime(), but the variable assignments would be clear. (and this one would have to throw if the binding included q vars). For that matter, the method you've written already can fail fast if the number of variables doesn't match.

Proposal: if you were interested/willing to do the placeholder variables now, that would be my first pick. But to move this forward, I would be ok adding the method with your originally proposed naming, and we add a TODO to replace these with placeholder variable variants in the near future?

Wdyt?

---

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

Previously, hongkai-dai (Hongkai Dai) wrote…

My rationale was that if we want to specify lb <= qdot <= ub, then it is easier to pass a BoundingBoxConstraint (which only requires specifying lb and ub, but not A), than passing A, lb, ub. Does that make sense?

I would think that lb <= qdot <= ub should probably be a special case that deserves it's own entry point, since we already have so many methods like that (e.g. AddPathVelocityConstraint). But I take your point that this could work for linearcontraint, linearequalityconstraint, and bounding box constraints.