Seeding solutions

[felixvd]

I have looked through the past issues but could not find any discussion on this, so I am opening a new one.

We have tested using MTC for our assembly competition, in which there are long tasks with multiple interdependent pick/place/regrasp operations. We found that although it is helpful to code the tasks in this manner, generating a solution takes a significant amount of time (1-2 minutes for the video below). Evidently that is too long to wait during execution.

A workaround that suggests itself would be to calculate one or more Solutions offline and use them to seed the search during execution, rather than starting blind. In the task pictured above, only the initial position of the object will change, so ideally only the initial Pick container would need to be adjusted at runtime, while the rest of the solution could be reused. I suspect that this feature would be helpful in other applications as well.

Have you had plans to supply seed Solutions for a Task? If yes, how would you implement it? I can imagine a few ways (e.g. a verifyTrajectory() function), but I don't know enough about the flow to judge.

[rhaschke]

Great application video! I'm not sure what you exactly mean by seed solutions.

• Of course, you could already now provide a stage that simply verifies an offline computed trajectory. If the trajectory is collision-free, you are fine and consider it as the solution. Otherwise, you could trigger a standard pipeline planner to come up with an online-computed solution. Obviously, this pattern calls for the fallback container.
• Alternatively, you could use multi-shot planners, e.g. PRM, which precompute the solution space and just adapt to the current situation at hand.
• Finally, just to improve planning speed, you could split the task into known sequences with fixed hand-over situations and plan these sequences independently of each other. Obviously, you loose the flexibility of MTC to dynamically adjust the hand-over states.

[v4hn]

Sorry, I missed the issue before. Nice work setting this up @felixvd !

"Seeding" solutions is not easily possible because the framework is too general for that, allowing to compute solutions with arbitrary code (that would need to be able to handle seeds).
As Robert said, it's possible to cache results in individual stages though (e.g. via multi-shot planners).

One other way to improve behavior in your case is to commit early to the first part of a solution once you have a successful way to pick the work piece and do not explore other ways to solve this anymore. This allows to start executing before the whole Task is solved and reduces planning time because there are less branches to explore.

[felixvd]

Thanks for the response. Feel free to use the animation as you like.

you could already now provide a stage that simply verifies an offline computed trajectory

Adding to the Task a Fallback container that first checks if an existing trajectory solves the problem would certainly work, but after that check, the online planning would start from scratch. We found that when planning online, we do not reliably get a good solution for this task - the best solution found within the first completed ones can differ a lot. It seems that locally favorable plans are found before globally good ones, and the "sensible" solutions (ones that a human operator would judge favorably) can take a while to appear. Tuning the parameters also only goes so far, and increases the calculation time.

To help the online planning arrive at a favorable path through the Task, our idea is to calculate solutions for a Task offline and to initialize the stages of the Task with trajectories from the seed solutions, so each stage (or most of them) would start the online planning with a guess from a successful past solution. The motivation is that if single stages fail on the initial "seed" trajectory (e.g. the initial pick pose of an otherwise good solution), only those stages would be recalculated "from scratch" until they can connect to the rest of the solution.

Alternatively, you could use multi-shot planners, e.g. PRM, which precompute the solution space and just adapt to the current situation at hand.

My understanding is that multi-shot planners would speed up the planning inside each individual stage, but would not allow this pre-loading with known-to-be-good Task solutions.

Finally, just to improve planning speed, you could split the task into known sequences with fixed hand-over situations and plan these sequences independently of each other.

That is always an option, but the goal of this effort is to avoid having to do that :)

edit:

Nice work setting this up @felixvd !

The glory should go to @karolyartur who just completed his internship here :)

As Robert said, it's possible to cache results in individual stages though (e.g. via multi-shot planners).

If you think what I described above may be possible to implement with multi-shot planners in general, that would be interesting. I'm all ears if you have a concrete suggestion. We could also discuss it on a call.

To clarify, I imagined that seeding the Task solution would result in initial connected sub-solutions (?), so that the final solution is less random. If only trajectories in each stage were cached, wouldn't the connection still be somewhat up to chance? The idea is to tell the planner "Hey try this way first, it worked out well before" with the seed solution and to avoid the randomness.

[rhaschke]

We found that when planning online, we do not reliably get a good solution for this task

That's the nature of sampling-based methods, like RRT.

To help the online planning arrive at a favorable path through the Task, our idea is to calculate solutions for a Task offline and to initialize the stages of the Task with trajectories from the seed solutions, so each stage (or most of them) would start the online planning with a guess from a successful past solution.

As stated by Michael, MTC is too generic to allow that - mainly because most sampling-based methods don't support such a seeding. I think, what you are looking for is an optimization-based planner (that can/must be seeded) like CHOMP or TesserAct.
You could use a CHOMP pipeline with MTC out of the box.

My understanding is that multi-shot planners would speed up the planning inside each individual stage, but would not allow this pre-loading with known-to-be-good Task solutions.

That's true. From this statement, I conclude that you actually want to optimize the InterfaceStates between stages. However, doing this also requires adaption of the stage solutions! The framework is not designed to do this.
If you want to "seed" your solution to certain InterfaceStrates, e.g. pick/place or hand-over poses, you could do so by seeding your appropriate Generator stages. However, you cannot seed a whole task solution.

[v4hn]

One thing I mean to work on - also in the context of scheduling heuristics - is to allow key properties of a stage computation (e.g. the approach angle) to be determined by the scheduling. As a result it could schedule combinations of these properties that worked well in the past instead of the "grid-search-style" that is currently employed. Still, this would compute at least one full run through the Task (with known key properties).

We could also implement something like a load method to load stored solutions into a Task / Container (after some verification), but there are a lot of corner cases to consider there and it would only really help if your new solution parts are exactly compatible with these stored solutions, e.g., the new solution to pick the work piece picks it at exactly the same angle as in the stored solution.

Any thoughts on these ideas @felixvd @karolyartur?

[felixvd]

That's the nature of sampling-based methods, like RRT.

Naturally – although I am not sure if this is mainly due to the random sampling-based planners, rather than how the generated subsolutions (?) connect in our Task. Most of our stages' motions are so small that planning succeeds with a direct joint motion.

As stated by Michael, MTC is too generic to allow that - mainly because most sampling-based methods don't support such a seeding. I think, what you are looking for is an optimization-based planner (that can/must be seeded) like CHOMP or TesserAct.

Our Tasks can branch such that two seed solutions might have very different subsolutions in the same stage, so supplying two very different solutions to an optimizing planner, or only a single solution for each stage does not seem like the right strategy. But I suspect I am misunderstanding, since I don't think optimizing the stages' motion plans is necessary in the first place. I only want to supply successful trajectories to stages where the seed solution works without changes

From this statement, I conclude that you actually want to optimize the InterfaceStates between stages.

Likely, and I might have been mistaken about the structure – my understanding was that each stage can store multiple potential solutions that it received from a planner. Looking at it again, it seems that Interfaces between the stages store InterfaceStates which store Solutions (the scene and their incoming/outgoing trajectory). Please correct me if I'm wrong.

However, doing this also requires adaption of the stage solutions!

Just so we are on the same page: I imagine that invalid trajectories (= ones that cause collisions) from seed solutions would be discarded, but feasible trajectories would remain in the pipeline. I am not sure why that would require adapting those stages' solutions.

What if the seeding process ran before the actual solution, and checked each trajectory for collisions before adding it to the relevant Interface? That would not require a planner, or any adapting of the stage solutions.

We could also implement something like a load method to load stored solutions into a Task / Container (after some verification), but there are a lot of corner cases to consider there and it would only really help if your new solution parts are exactly compatible with these stored solutions, e.g., the new solution to pick the work piece picks it at exactly the same angle as in the stored solution.

It sounds like we are thinking of the same thing here. I would discard all the trajectories that are not exactly compatible, but leave the InterfaceStates (or Solutions inside of them?) so that they can be integrated into the final solution.

To reuse the concrete example at the top: If the workpiece was initially at a different location, and the solution pictured in the OP was supplied as a seed solution, I would expect that all its trajectories except some for the first Pick container would be valid. Then one of the solutions generated in the Pick container should connect to the available loaded solution at the next Connect stage fairly quickly (assuming that the same grasp pose is achievable).

It is true that the grasp candidates/poses need to be exactly the same, you're right. Let us assume that.

One thing I mean to work on - also in the context of scheduling heuristics - is to allow key properties of a stage computation (e.g. the approach angle) to be determined by the scheduling. As a result it could schedule combinations of these properties that worked well in the past instead of the "grid-search-style" that is currently employed. Still, this would compute at least one full run through the Task (with known key properties).

By "computing a full run through the Task (with known key properties)", you mean that all necessary stages of the task need to be solved by the planner, even though the key properties (e.g. generated grasp and place poses) may be "ideal", correct?

I like that you mentioned combinations of key properties, since e.g. good handover poses will depend on the robot, and they might be quite different between your PR2 and our two UR5 arms. A heuristic like that could represent the robot's knowledge about its embodiment and its capabilities, which I find very cool!

What seems a bit cumbersome is that a) you need to calculate the whole task, as you said, and b) it seems like one would need to train the heuristic, and that might not carry over well to different items and tasks. Training a heuristic that works in many situations would be interesting for general purpose robots in unstructured environments. But when you know this much about the task ahead of time, it seems like seeding solutions would be easier (to implement, to teach and to confirm that it works as expected).

As background: For this application (small lot sizes of 50-1000 units), spending some computing time to calculate good seed solutions would be very feasible, but manually setting up all the handover poses etc. for each unit would be prohibitive. Using Tasks to assemble primitives for these operations is an attractive prospect, and if this seeding feature can make them faster and reliable, I would definitely want to investigate it.

[rhaschke]

Likely, and I might have been mistaken about the structure – my understanding was that each stage can store multiple potential solutions that it received from a planner. Looking at it again, it seems that Interfaces between the stages store InterfaceStates which store Solutions (the scene and their incoming/outgoing trajectory).

Yes, solutions are associated to InterfaceStates, because they are interlinked to the start/end state of a particular InterfaceState: You cannot use any solution of a stage with any InterfaceState.
Of course, each stage also has a list of all its own solutions (used for rviz visualization).

Just so we are on the same page: I imagine that invalid trajectories (= ones that cause collisions) from seed solutions would be discarded, but feasible trajectories would remain in the pipeline. I am not sure why that would require adapting those stages' solutions.

I'm afraid we are not on the same page at all. Maybe we should switch to a zoom call instead. However, I'm afraid I don't have the time this week for this.

[v4hn]

Just so we are on the same page: I imagine that invalid trajectories (= ones that cause collisions) from seed solutions would be discarded, but feasible trajectories would remain in the pipeline.

You could implement something like that, but it's not as easy as you think.

What Robert means to say is

• the stages themselves validate feasibility when generating solutions. There's no external mechanism on top
• a solution (e.g. a SubTrajectory) is already bound to a start- and end-PlanningScene. So they are either always valid or never
  If you want a mechanism to generate previously computed solutions for varying PlanningScenes, you will generate a new SubTrajectory for each such solution.

I'm also open to have a call (also to hear some more internals about the Task you set up) but my time is scarce..
How about a short round before/after the maintainer meeting on Thursday?

[felixvd]

Thanks for the explanation. It is helpful.

You could implement something like that, but it's not as easy as you think.

I don't imagine it's easy :) Just a very attractive performance gain.

the stages themselves validate feasibility when generating solutions. There's no external mechanism on top

I assumed as much. I also assumed that the bulk of the work would be in implementing the methods you described to validate "loaded" solutions.

• a solution (e.g. a SubTrajectory) is already bound to a start- and end-PlanningScene. So they are either always valid or never
  If you want a mechanism to generate previously computed solutions for varying PlanningScenes, you will generate a new SubTrajectory for each such solution.

I am not sure how to interpret "they are either always valid or never" (when would "never" occur?), but I assume you mean that currently, all solutions in an Interface are considered valid (since the planner would not have generated it otherwise), and there is no concept of validating them. I also understand you to mean that one could consider the loading/validating to be "generating" a solution from previous data, which I agree might be a good approach.

Anyway, this will be easier to discuss on a call! How about an hour before the maintainer meeting, tomorrow 16:00 GMT+1? I'll be more awake before than after.

[v4hn]

I don't imagine it's easy :) Just a very attractive performance gain.
I assumed as much. I also assumed that the bulk of the work would be in implementing the methods you described to validate "loaded" solutions.

It should be relatively straight-forward to implement such a store-load system for solutions because they can already be serialized. I don't think you would need to validate anything as long as you don't manipulated the stored solutions yourself.

I am not sure how to interpret "they are either always valid or never" (when would "never" occur?), but

"never valid" -> if the trajectory of a solution is in collision with the start InterfaceState's PlanningScene.
As the scene will never change, the trajectory will never be valid.
The stages can definitely spawn such "invalid" solutions and they do!
Almost always they will mark them as invalid (infinite cost) though(!) and the system will not attempt to compute other stages based on them. If you expect that execution of a specific subsolution will be handled by a custom routine that does unmodeled things anyway you might spawn an invalid solution with a valid cost too...

I also understand you to mean that one could consider the loading/validating to be "generating" a solution from previous data, which I agree might be a good approach.

This is pretty much what I usually summarize as a "spline database solver" you could implement for MoveTo/MoveRelative stages. But the approach is more general than just motion splines as the scene might be changed as well.