Custom Costs and Constraints for GcsTrajectoryOptimization

[cohnt]

#21179 added the ability to specify which transcriptions a cost or constraint in GraphOfConvexSets is used for. This functionality would be useful in GcsTrajectoryOptimization, but the various AddCost... methods do not allow you to specify the transcription. Semi-related to #21460?

[rpoyner-tri]

Assigning component owner @hongkai-dai for further investigation.

[hongkai-dai]

I am less familiar with GcsTrajectoryOptimization. I think @cohnt and @wrangelvid know that code better than I do. Re-assigned to @cohnt

[cohnt]

Happy to take this one on -- just not sure when I'll get to it.

David, are you alright if I just match the interface used in GraphOfConvexSets -- taking in a std::unordered_set<Transcription>?

[wrangelvid]

David, are you alright if I just match the interface used in GraphOfConvexSets -- taking in a

Can you elaborate on an example use case? I think this can be quite a footgun if not used right... e.g. Adding a time minimizing cost to the relaxation, but then minimizing path length in the restriction could make the relaxation arbitrary loose.

[cohnt]

If I want to use a richer nonconvex objective for the rounding stage, but a convex surrogate for the relaxation, we don't have an easy way of doing that yet.

Another thing I've experimented with is using an L1 cost for the relaxation, and an L2 cost for the rounding. That way, you end up with an LP instead of an SOCP, which seems to be faster and use less memory (important for really big graphs).

[wrangelvid]

I see. It makes sense for a generic AddCost function.

If adding an L1 path length cost to the relaxation becomes the standard way, perhaps invoking it through an option in the AddPathLengthCost yields a better developer experience?

[RussTedrake]

I'm also a little worried that we need to be careful with what APIs we offer here. Of course, one can always get the underlying GCS object and work with that.

If I want to use a richer nonconvex objective for the rounding stage, but a convex surrogate for the relaxation, we don't have an easy way of doing that yet.

I don't think we actually offer any nonconvex objectives yet, do we? For the nonconvex costs, the philosophy is that when calling e.g. AddContinuityConstraints, we already and automatically add the convex surrogate for the relaxation and the nonconvex for the restriction + MIP.

Another thing I've experimented with is using an L1 cost for the relaxation, and an L2 cost for the rounding. That way, you end up with an LP instead of an SOCP, which seems to be faster and use less memory (important for really big graphs).

That's pretty specific and special case. We could potentially also achieve that by making sure folks can remove the costs that get added.

I think we need to balance giving people modeling power here vs protecting people from misusing the tool. GCS is more general, GcsTrajOpt, in my mind, is more about taking people who might not be GCS experts and giving them the modeling tool at the power of trajectories that will protect them from leaving the costs and constraints that we support properly.

[cohnt]

Although we can get the underlying GCS object, we can only get it as const, so I don't think we can add these sorts of costs with the current codebase. Making GCS non-const seems difficult, since you could very easily break what GcsTrajOpt requires. (One also would need to know how GcsTrajOpt is storing variables in the sets -- I'd have to check the code to see if the time scaling is first or last.)

A generic AddCost function that supports specifying kTranscription would definitely give me what I need. Do you (David or Russ) have any thoughts on how we might add that to give us the modeling freedom we're looking for, while still preventing the user from shooting themselves in the foot? Perhaps separate methods AddCost and AddConvexCost, where we can make stronger guarantees in the latter case? (And analogously for constraints?)

[RussTedrake]

Perhaps this requires a whiteboard conversation. But I might prefer something more along the lines of:

GcsTrajectoryOptimization::Subgraph::AddPathPositionConstraint(
     const std::shared_ptr<solvers::Constraint>& constraint, 
     const std::shared_ptr<solvers::Constraint> & convex_surrogate,
     double s)

or similar, because it would emphasize the intended workflow.

Note that I chose that particular example based on trying to maintain some consistency w/ AddPathPositionConstraint in KinematicTrajectoryOptimization.

[wrangelvid]

or similar ...

I have a very similar implementation for ‘AddPathPositionConstraint’. The only difference is that I have an additional argument for the desired vertex to add the constraint to. Sometimes, the discretion distance s can be varied based on the properties of the region (for instance, the maximum distance).

Are you suggesting we create a similar interface for costs, such as ‘AddPathPositionCost’?

[RussTedrake]

Sometimes, the discretion distance s can be varied based on the properties of the region (for instance, the maximum distance).

s should always be in [0, 1], so this specification should be robust to scaling, no?

Are you suggesting we create a similar interface for costs, such as ‘AddPathPositionCost’?

If @cohnt is saying that he needs this sort of thing, then yes?

[cohnt]

I'm not quite sure something providing a similar interface to KinematicTrajectoryOptimization's AddPathPositionConstraint would work, since I specifically need to compare two subsequent control points. It seems like there's two distinct types of costs you might want to add -- a cost which is only applied at a specific point along a trajectory, and a sort of path integral cost.

This seems to be bumping into a broader question of how to add modeling freedom to GcsTrajOpt without leaving landmines for unaware users. Maybe we can spend some time at the next GCS standup discussing?

[cohnt]

Here's what we came up with as a result of today's discussion. Russ, please correct me if any of this doesn't match what you were thinking.

At the minimum, we add methods roughly looking like:

• GcsTrajectoryOptimization::Subgraph::AddConvexCost(const std::shared_ptr<solvers::Cost>& cost, const std::unordered_set<Transcription>& use_in_transcription)
• GcsTrajectoryOptimization::Subgraph::AddCost(const std::shared_ptr<solvers::Cost>& cost, const std::unordered_set<Transcription>& use_in_transcription)
• GcsTrajectoryOptimization::Subgraph::AddConvexConstraint(const std::shared_ptr<solvers::Constraint>& constraint, const std::unordered_set<Transcription>& use_in_transcription)
• GcsTrajectoryOptimization::Subgraph::AddConstraint(const std::shared_ptr<solvers::Constraint>& constraint, const std::unordered_set<Transcription>& use_in_transcription)
• GcsTrajectoryOptimization::EdgesBetweenSubgraphs::AddConvexConstraint(const std::shared_ptr<solvers::Constraint>& constraint, const std::unordered_set<Transcription>& use_in_transcription)
• GcsTrajectoryOptimization::EdgesBetweenSubgraphs::AddConstraint(const std::shared_ptr<solvers::Constraint>& constraint, const std::unordered_set<Transcription>& use_in_transcription)

The costs would be applied to each vertex in the Subgraph, and the constraints would be applied to each edge in the Subgraph (or EdgesBetweenSubgraphs). We would check that the given costs and constraints matched the number of variables in a given vertex/edge, although down the line, we could have some syntactic sugar using placeholder variables.

For the convex cost and constraint methods, we could make guarantees that the underlying GCS object would be able to handle what was being asked, but the arbitrary ones would make no such guarantees. Note that GCS currently uses RTTI to add the associated perspective cost/constraint to the relaxation, so perhaps we don't need to check compatibility here (since GCS will already check it).

This would remove the need to specify the transcription in existing methods to add costs and constraints, since the user can go in and manually add the costs to the transcriptions they care about.

[RussTedrake]

Thanks @cohnt for the summary.

We would check that the given costs and constraints matched the number of variables in a given vertex/edge, although down the line, we could have some syntactic sugar using placeholder variables.

I disagree here. I think we want to put the placeholder variable idea in place first. Expecting the constraints to depend on all of the decision variables and match the number + order of the subgraph/EbS storage would be too fragile, I think.

[cohnt]

I want to check that I understand the placeholder variables correctly -- is the idea that we would have a method like GcsTrajectoryOptimization::Subgraph::GetPlaceholderXu which effectively plumbs through GraphOfConvexSets::Vertex::x() for a generic instance of a vertex within the subgraph? (Perhaps two methods, one to return the position variables, and one to return the time scaling variable?)

[RussTedrake]

is the idea that we would have a method like GcsTrajectoryOptimization::Subgraph::GetPlaceholderXu which effectively plumbs through GraphOfConvexSets::Vertex::x() for a generic instance of a vertex within the subgraph?

That's exactly right. But I would just call it GcsTrajectoryOptimization::Subgraph::x().

[cohnt]

Got it! I can probably start making some progress on this. Presumably, we would also want methods like

• Subgraph::xu() and Subgraph::xv() for adding edge costs and constraints within a subgraph
• EdgesBetweenSubgraphs::xu() and EdgesBetweenSubgraphs::xv() for adding edge costs and constraints between subgraphs
• Similar methods to get out the time scaling variables?

[RussTedrake]

Let's see. Subgraph is an abstraction on top of a Vertex, but with a particular ordering of the variables. EdgesBetweenSubgraphs generalize the Edge. For the names, I would match KinematicTrajectoryOptimization

So I guess I would think:

• Subgraph::control_points()
• Subgraph::duration()
• EdgesBetweenSubgraphs::control_points_u() (and again for v)
• EdgesBetweenSubgraphs::duration_u() (and again for v)

or u_duation something like that?

[cohnt]

I can do something like that. But a subgraph also implicitly adds edges between its constituent sets, so we'd need analogous methods to grab placeholder variables for the edges within a subgraph. I'm also thinking maybe the edge methods return a pair of variables, so we don't need to have two separate functions to get the incoming and outgoing variables.

[cohnt]

Making a TODO list here to actually track the progress

• Subgraph costs and constraints
  • Expression and Formula versions Add Expression Costs and Formula Constraints to Subgraphs in GcsTrajectoryOptimization #22091
    • Python bindings Python bindings for methods of GcsTrajectoryOptimization::Subgraph for adding generic costs and constraints. #22156
  • Bound cost/constraint versions (including python bindings) Add Generic Binding<Cost> and Binding<Constraint> to vertices and edges within a GcsTrajectoryOptimization::Subgraph #22179
• EdgesBetweenSubgraphs costs and constraints
  • Expression and Formula versions Add Expression Costs and Formula Constraints to EdgesBetweenSubgraphs in GcsTrajectoryOptimization #22155
    • Python bindings Python bindings for methods of GcsTrajectoryOptimization::EdgesBetweenSubgraphs for adding generic costs and constraints. #22177
  • Bound cost/constraint versions (including python bindings) Add Generic Binding<Cost> and Binding<Constraint> to vertices and edges within a GcsTrajectoryOptimization::EdgesBetweenSubgraphs #22199