Planning slowdowns and logging freezes in long tasks

[DaniGarciaLopez]

I realized that long tasks were producing less and less logging during planning. At a certain point in the terminal, log messages from the planning requests become scarce, and eventually they stop completely. The same happens in the Rviz panel.

I created a small demo to reproduce this (here). It’s just the pick/place demo modified by adding all stages to a container and repeating them several times. In each stage I call addSolutionCallback, so every time a new solution is created (success or failure) I record its timestamp. After planning finishes, I plot how many new solutions were created per second to get an approximate understanding of how the planning evolved.

I would expect the rate of new solutions per second to remain approximately constant. This is true if you repeat the Pick/Place sequence 8 times or fewer on my computer (you can modify it here). However, with 9 repetitions you can see that at some point (after 35 seconds) the planning completely stops: there is no more logging in the terminal, it pauses for about 3 seconds, and then it resumes and quickly finishes. Here is the graph:

With 10 repetitions this becomes even more noticeable. The number of new solutions created slows down after second 30; at second 45 it stops completely, and then it resumes only after 250 seconds with no logging at all (even in debug), which makes it look like something crashed. It produces a peak of 20,861 solutions in second 307 and then finishes. Note that I had to set the vertical axis to a log scale because of that peak; otherwise nothing else was visible. If I simply sum all new solutions and divide by the total planning time, I get a much smaller average rate than when planning with fewer repetitions, so a slowdown in planning must be happening.

I initially thought this might be a resource issue on my computer, but CPU, RAM, and disk usage remain well below their limits during planning. Also, the results are very consistent every time I run the demo. I tried with 11 repetitions and never got a result in a reasonable time—the logging stops entirely at some point. Rviz shows that all the waiting time was actually spent inside a single MoveTo stage.

I’m aware that a task like this is not very realistic, since it would be faster to plan just one Pick/Place at a time and execute them sequentially. But this was simply an easy way to illustrate the problem we’re experiencing with very long tasks when you’re unable to split them into simpler ones.

Is there a bug here? I only tested this using MTC and Moveit from the jazzy branch.

[rhaschke]

I think you are running into an exponential increase of solutions: when a new solution is found by a child of a serial container, all possible solutions of the container involving this new sub solution will be enumerated by the container. If you plot the number of overall task solutions against the number of repeated pick-place-containers, I'm pretty sure you will see this exponential increase.
For example, in the task below, there exist 3 different solutions connecting the unique start and end states. If this pipeline is sequenced N times, this will result in 3**N solutions!

To improve on this, we might want to limit this enumeration. Typically, we are only interested in the best solution.
@v4hn: I guess you already noticed that issue in your PhD thesis. Do you, by any chance, have a solution already?

[DaniGarciaLopez]

I agree that this is a big problem for long tasks. I plotted it and looks pretty much exponential.

I had some ideas in mind, such as adding a stage between pick/place containers that takes only the best solution from the first container and uses it to seed the second one. If the second container fails, it would then seed the second-best solution from the first container, and so on. I didn’t have time to look into how difficult this would be to implement right now. Maybe, as you said, we can handle this directly in the container without adding extra stages.

There’s also another problem, because of the depth-first search nature of the exploration. The first containers always end up with much more optimized trajectories than the last ones, since many more solutions are computed for them. Something like task->plan(5) is not really meaningful here because you end up with hundreds of thousands of valid solutions. In this case, I think it would be better to set a minimum number of valid solutions per container, to ensure that the last containers are optimized enough.

The main problem I see—and the reason I opened the issue—is that at some point it completely stops creating new solutions. What is it actually calculating at that moment? Is it enumerating all combinations? At first I thought it had crashed, because there’s no indication that it’s doing any work in the background; there’s no logging for several minutes. If this is not a bug, then I think we should at least print some messages indicating that it’s still working, so the user doesn’t think it crashed.

[v4hn]

I agree you probably notice combinatorial blow-up there that you want to avoid either way.

Typically, we are only interested in the best solution.

Not quite. You want to maintain at least the best one for each set of co-parameter vectors at the open interfaces of solution-sequences or you drastically loose control over the solution space.
Intuitively, if you can have a container solution that moves an object and one that does not move the object, the cheaper one is probably the one that does not move it, but that doesn't mean that your whole task can be satisfied like that.
Here, co-parameters are mostly all object poses/grasps, but if you want to ensure the optimal (sampled) solution possibly also the joint states. I started implementing co-parameter projection for solutions in some parts, but did not get to a functional prototype before.

Do you, by any chance, have a solution already

I usually do not use such very long multi-solution sequences and would split them up with solution commit.
So either you limit the number of container solutions to one to avoid alternatives in the first place or you plan the containers as separate tasks independently and commit to the best solution in each container.

Another point where this blow-up comes in later during solving is with the monitors in your example:
https://github.com/DaniGarciaLopez/moveit_task_constructor/blob/57fb75f05156f5b683cabc29b6e2c99a80052fd4/demo/src/pick_place_task.cpp#L204
As you always monitor the first stage of each container, your generators further down the line keep solving the same IK requests countless times due to the forwarded solution sequences.
That particular part can also be avoided by seeding them from a much earlier stage in the beginning of the sequence if you can fully predict the intermediate states anyway.
But that's not what you measured there.

[v4hn]

What is it actually calculating at that moment? Is it enumerating all combinations?

exactly that. onNewSolution in containers counts to the compute time of the last child stage that yields the connecting solution.

[rhaschke]

onNewSolution in containers counts to the compute time of the last child stage that yields the connecting solution.

Maybe, we should change that?

[DaniGarciaLopez]

As you always monitor the first stage of each container, your generators further down the line keep solving the same IK requests countless times due to the forwarded solution sequences.

Thanks for pointing this out! I wasn’t aware of it, and it might be the key to optimizing some of our tasks. I’ll give it a try.

limit the number of container solutions to one to avoid alternatives

Any hints on how to do this? Is it currently possible?

[rhaschke]

limit the number of container solutions to one to avoid alternatives

Any hints on how to do this? Is it currently possible?

I don't think that's possible right now, but not too difficult to implement. However, I don't have time to work on it soon.
Have a look at #730.

[DaniGarciaLopez]

Have a look at #730.

Just tested it, huge improvement!!! It looks like the sorting there was slowing things down a lot. The planning time with 10 repetitions is now ten times faster, and I can get results with 11 repetitions in a reasonable amount of time.

However, I think we should add some logging in case it stays in that loop for too long. Maybe we could check whether it has remained in that loop for a few seconds and, if so, print the progress of incoming solutions every couple of seconds as they are being collected.

[rhaschke]

Interesting. I wasn't expecting a speedup, but just aimed for a more continuous solution reporting.