Shortcutting multi-robot multi-goal plans

In multi-robot path planning, it is often relatively easy to find a bad path, since we can just move robots out of the way if we control all of them. It is usually much harder to find good paths. So what we do is find a bad path, and then postprocess it into a good path. This is standard practice in path planning, and is called (partial) shortcutting. It is unclear how to best apply this to multi-robot multi-goal path planning, because we need to shortcut over the fully constrained parts as well.

This is more or less a writeup of the work I did recently for the multi-robot multi-goal multi-modal path planning (which was published at WAFR this year), respectively the multi-robot assembly planning work (where I improved a bit upon the initial implementations of the shortcutters we came up with for the initial work).

I decided that it might be interesting to have a standalone post that focusses on the shortcutting part, and explain it a bit in more detail what it does and why it’s good. The other goal I have with this post is setting up a benchmark, where people could build uponAnd I ended up doing better analysis than before, and tested a few small new ideas. . This would benefit me since the paths that I would be getting in several applications would be better faster.

In the following, I am not going to explain how basic shortcutting works. This can be found in various places, e.g., here.

The code for all experiments in the followin gcna be found in the repo hereDisclaimer: The code was basically entirely generated by Coding Agents, and I only checked that it actually does what it claims. The code as is is not pretty, but the organization is relatively easy to follow. .

Shortcutting for multi-robot multi-goal paths

As mentioned above, I am interested in multi-robot multi-goal multi-modal path planning, and as part of it in post-processing of those paths. The path planning side of it boils down to finding a feasible path in a high-dimensional space. This space has a special structure: it is a cartesian product of all the separate single robot configuration spaces, i.e.

\[\mathcal{Q} = \mathcal{Q}_1 \times \mathcal{Q}_2 \times ... \times \mathcal{Q}_N\]

This can and should be used to plan more effectively, but we are for now not really doing that. The nice thing about ignoring the structure, and just treating everything as a single robot is that we can easily get optimality guarantees by using existing planners.

In the following, we are then going to treat the shortcutting problem as a setting where we shortcut a given initial path with equality constraints (i.e. the end points of each ‘mode’) only on parts of its dimensions. Any changed path still needs to satisfy these equality constraints, i.e., our path is required to go through certain parts of the configuration space.

Below, we see these constraints illustratedTaken from the paper. :

Here, the modesA mode of a robot is effectively the state that it is currently in, e.g. “grasping a box” or “setting down a box somewhere”. of the robots are shown with the grey boxes, and if a box ends, we require a robot (one of the disks here) to be in a very specific position (e.g., in order to pick something up, or for example to do a point-weld). The red dashed lines show the points at which we have a mode switch of any robot (and thus the overall mode changes).

The key realization is that only part of the cartesian product that makes up the composite configuration space is constrained:

\[\mathcal{Q}_\text{constrained} \subseteq \mathcal{Q}_1 \times \{q_2\} \times ... \times \mathcal{Q}_N.\]

So we can still freely shortcut large parts of our full composite space, just not the constrained robot coordinates. This means that we can do:

• As the simplest baseline (called ‘full configuration’ below), shortcutting in the full configurations space. This means that we can never shortcut over mode-boundaries/the equality constraints (again, since we have a hard equality constraint where we need to be with the mode switching robot).
• As next step up (called ‘random robot’ below), we can choose single robots at random, and shortcut its dimensions. In here, we also need to make sure that the indices we choose to linearly interpolate between are in the same mode.This is what we originally did in the multi-robot multi-goal multi-modal path planning work. This has since been replaced though, with something we will see below. But that already means that we can shortcut longer paths compared to considering all robots at the same time.
• Instead of single robots, we can also choose groups of robots to shortcut (called ‘random robot subset’ below). The rest is the same, but we now need to make sure that all modes of the robots that are part of the subset do not change.

In the approaches above, the simple implementation of shortcutting is ‘choose two random indices of the path, and hope that there is no mode switch in between’ (denoted ’+ endpoints’ below). Instead, we can also sample the mode that we shortcut directly, and then choose the indices only on that part of the path (denoted ’+ compatible run’ below).

Baselines

I am going to give details on the experiments that we do below in the experiments section, but for now, here are some plots, showing how each approach does on a variety of problems, for shortcutting only (i.e., not feeding the paths back into the planner again). These plots for selected scenarios show time on the x-axis, and the cost of the path on the y-axis:

Plots with a logarithmic time axis

The main thing to take away here is that it is clearly very useful to just sample the mode to shortcut directly. Compared to what we did in the WAFR paper, this surprised me: I was assuming that the rejections of invalid index-pairs (i.e. pairs that cross mode boundaries) are more or less freeHonestly, seeing these plots here, it is almost embarrassing to have used the naive version in the paper originally. .

But this is explainable: If we just sample endpoints randomly, we have a relatively low probability of actually sampling both points in a constant mode part (as shown in the data below), and this probability decreases with the number of robots, and with the number of different tasks that we doSo effectively, we just have very bad chances of finding a shortcut that we can even attempt. And because of how I implemented things in the planner, there is a budget of attempts that can be made - thus if we waste them with too many invalid attempts, we end up with a worse method. .

Shortcut lengths and acceptance rates

Environment	Sampling	Accepted span median / 90th percentile	Compatible proposals	Accepted proposals
Box rearrangement	full configuration	2.0% / 3.3%	2.6%	0.01%
	robot + endpoints	8.6% / 20.4%	15.6%	1.23%
	robot + compatible run	6.6% / 19.7%	55.3%	1.26%
Car assembly	full configuration	1.9% / 2.9%	1.9%	0.02%
	robot + endpoints	12.1% / 29.1%	38.3%	7.23%
	robot + compatible run	3.9% / 16.5%	54.5%	2.66%

Medians over three 30-second runs. Shortcut spans are percentages of the initial path; proposal percentages are relative to all sampled proposals.

To me the other surprising thing here is how good full configuration shortcutting is, and especially how quickly it leads to improvements, especially given how few of the attempts are actually feasible due to crossing modes.

One last thing to point out is that even though full configuration shortcutting is improving the path very fast initially, it also does not converge to the same best cost as the others, since we will always keep the waypoints that coincide with the mode-switches.

However! As said above already, these plots show pure ‘offline’ shortcutting. In the context of the multi-robot planners, this is not the most realistic setting, as we insert the shortcuts back into the tree/roadmap, and continue working with the new plans. This changes the plots:

Plots with a logarithmic time axis

In these plots, I added the planner without shortcutting as baseline comparison to show that shortcutting is actually useful.

And this shows that the difference is not actually that big!So luckily saving face a bit for me. We see that the algorithm that is best in the offline setting is also best in the online setting, but it is all much closer than before. Now, this is partially due to a relatively small shortcutting budget, and partially due to the fact that the reinsertion of the plans into the planner dampens some of the bigger differences from the pure offline setting.

We could optimize our shortcutting approach for either of these settings, but I do believe that in the end we want one algorithm for both postprocessing, and ‘online’ use.

Prior work

There is not much work on multi-robot multi-goal plan-postprocessing, so I am just going over a couple of things that I used for ideas and inspiration.

• Partial shortcutting: I wrote about this in the post here a bit. The core idea is that instead of just sampling indices on the path, and linearly interpolating them, we sample indices on the path and (a subset of) dimensions which we linearly interpolate. Crucially, the other dimensions are kept as they were before (i.e., the path is not replaced with the straight line interpolation for those dimensions). This carries over very naturally to the multi-robot case, as we can just sample a subset of robots, or a single robot and only shortcut that one. This is what we already introduced above.
• There was some work presented at WAFR this year on shortcutting: It was slightly more on the theoretical side, but also suggested some approach that we’ll see below is surprisingly similar to something I converged to below. The core idea is that we fix the length of the shortcut we do, and move this window over our path, and then repeat this with a smaller window. After, we fall back to shortcutting with indices coming from a Halton sequence. The paper is cool, and worth looking at just for the plots alone already. They do unfortunately not look at partial shortcutting at all, which is what I usually do.
• At IROS ‘25, there was a paper on multi-robot shortcutting from Huang et al: They propose three approaches, and suggest that a round-robin style application of those, respectively estimating online which method has the highest probability of giving you a better path, works best.

I implemented a couple of those algorithms, and compared them against our baseline. Since none of the approaches are presented such that they apply to the multi-robot multi-goal setting, we make some adjustmentsIn the plots above we can already see that there is effectively no point on running the shortcutters over the full path at once – the planners that explicitly take into account where we can actually shortcut are always better than their sibling versions that don’t. :

• We test two versions for the WAFR shortcutter:
  • one that shortcuts full configuration modes directly, and is the same otherwise, and
  • one that takes the same endpoint sequences from the paper, but does that for single robots that are sampled randomly.
• The meta algorithms from Huang, round-robin and Thompson sampling do:
  • Sample full configurations and compatible modes, and within the modes, we sample endpoints uniformly.
  • Sample single robots, and compatible modes, and within the modes, we sample endpoints uniformly.

Compared to the original version from Huang, we do not do the prioritized shortcut in the following experiments, as this would imply re-checking the whole following trajectory. We did test this, and the shortcuts were rarely accepted due to collisions of the following path.

We compare these against our baselines on the selected scenarios, both offline:

Plots with a logarithmic time axis

and online:

Plots with a logarithmic time axis

We see again that the full configuration shortcutting from the WAFR paper has the same issues as the simpler full configuration shortcutting.

Generally, all the things are very similar to each other now as long as we do direct sampling of the modes that we shortcut, and not random endpoint sampling. In the offline setting, the round-robin style approaches tend to perform slightly better than others, but the full configuration shortcutting still has the fastest initial decrease of the cost. The other thing we can see here is again that in the online setting, the differences are not very big between the different algorithms.

Improving upon the baseline and prior work

We have seen that the version that randomly chooses a mode for a robot is performing best among the non-meta-algorithms we have briefly described above. Now, we can try to improve upon the things we are already doing, and take some inspiration from the other introduced methods.

We can not improve what we can not measure. So we start by measuring and generating some statistics from the random-robot subset + compatible runThese numbers are obviously going to be different for all methods, but we are just going to assume that this is good enough information for now. :

• How much time do we spend checking invalid edges, and how much time do we spend verifying edges that are actually collision free
• Which of the shortcuts that we do actually end up in the final path.
• How much do the shortcuts improve the path: could it be that we get the main improvement from only a few initially very suboptimal segments.

Environment	Candidateschecked / accepted	Check timevalid / invalid	Partially retained shortcuts	Improvement from top 10 shortcuts
Random 2D	13,637 / 2,742	85.3% / 14.7%	5.4%	34.7%
Box stacking	6,068 / 1,826	86.0% / 14.0%	9.5%	43.6%
Car assembly	3,495 / 1,151	82.9% / 17.1%	14.5%	61.3%

From this data, we see that

• Not that many shortcuts end up in the final path: We spend a lot of time ‘overwriting’ previously checked edges.
• We spend much more time verifying valid edges, than rejecting invalid proposals. Rejecting invalid proposals is fast! I want to emphasize this again: Taking this and the previous point together means that we spend a lot of time validating shortcuts that do not do anything in the end!Technically, they still do something because they alter the path, which leads to other shortcuts being feasible. But they do not contribute directly in the end.
• The best few shortcuts are responsible for a big part of the path improvement.

So effectively, what we should try to do is

• find the ‘final’ optimal long shortcuts directly, to avoid shortcutting the same indices many timesThat being said, I realized at some point that shortcutting single robots at a time also leads to collision checking indices multiple times, as we always check the whole scene, even if we only change a single robot. I have some experiments lying around that do smarter collision checking if we only do partial shortcuts, but I am going to omit this for this post. . However, this is very close to just stating ‘Well, we should just find the best path directly’.
• allocate our effort to the right modes, and not the ones that we did already shortcut to optimality before.

Shortcutting as resource allocation

We can see shortcutting as a resource allocation problem: we need to allocate the time we have for edge checking smartly in order to get as much improvement out of the time as possible. If we just spend the time carelessly, we might not improve at all, or not as much as we could.

In the multimodal setting, this could happen due to various reasons:

• we might not be able to improve the mode that we are in for a robot much further, or
• we might actually improve the path, and spend a lot of time verifying that the edge is collision free, and then later replace this edge completely with a better shortcut.

Both are not what we want! So with this in mind, we can try to design an algorithm that

• does shortcuts that are long, since those are more likely to end up in the final path
• shortcuts in modes that still have potential to be improved.

But we also want to make sure that we do not try the same shortcut time and time again.

Trying to predict shortcuttable modes

The things we describe above mean that we basically try to predict shortcuts that maximally improve the cost, and have a high likelihood of actually being collision free. If we write this up in the most general sense, this is very close to just predicting a path, which is not something we want to doStating it more plainly: This is just the motion planning problem itself. .

So what we settle for is updating our belief online based on attempted shortcuts to try to decide which robot-mode pair might be good.

We do this by taking two signals into account:

• How often we previously successfully shortcutted this robot/mode combination, and
• an estimation of how much further we could reduce the cost, by assuming that we could fully straighten the path.

Using this, we compute a score

\[s_a = \frac{g_a\,\frac{1 + A_a}{2 + P_a} + g_\mathrm{floor}}{1 + U_a}.\]

Here, an arm \(a\) is a robot (or robot group) and a compatible mode interval, \(g_a\) is the estimated remaining cost reduction, \(A_a\) and \(P_a\) count accepted and proposed shortcuts, and \(U_a\) counts how often the arm was selected. The gain floor is a small fraction of the mean gain and prevents arms with no estimated gain from being ignored completely.

We rank all possible arms using this score and sample shortcut endpoints uniformly within the highest-ranked interval. This approach can also be extended to dealing with subsets of robots, but I am not writing this out here.

We’ll refer to this method as ‘gain’, or ‘group gain’ (if applied to multiple robots) below.

A principled approach to long shortcuts

The other thing we want to do is a sensible approach to long shortcuts: An idea that we can have here is similar to a binary search:

• We attempt a full shortcut of a mode for a robot
  • If we are successful, we are done with this mode for this robot
  • If not, we split the mode at the index where the first collision was found, and add these two possible new ranges (start to index, and index to end) to a queue of shortcuts that we attempt.

We do this for a certain number of splits, and then do a fallback strategy in this approach. A fallback could e.g., be the method described above, or could also be just random sampling of endpoints within a mode.

The important thing to note here for the two methods is that they are stateful. This will have some implications when running this in the planner, where we restart shortcutting repeatedly.

We’ll refer to this method as ‘tree’ below.

Experiments

In the following, we will always show normalized cost improvement, and show a (more or less) representative sample of four scenarios. The rest of the plots will be available as well though.

The cost we are minimizing is an approximation of the makespan of the path, regularized with the path-length. We do this regularization in order to try to guide the plans out of the large nullspace that the makespan paths have:

\[C(\pi) = \sum_i \left(\max_r ||q_{i+1}^r - q_i^r||_2 + 0.01 \sum_r ||q_{i+1}^r - q_i^r||_2\right)\]

We are going to do two types of experiments, which we have already shown above:

• we will be running shortcutting on a dataset of paths that we generate ‘offline’, and measure how much we improve the initial cost of the path.
• We are also going to be running the shortcutters as part of the planning loop, in order to see how they change the planning performance.

The setup for both these things is available in the repository here. We heavily build upon the github repository from the multi-robot planning work, and generate initial paths for multiple environments from there.

In the experiments, we make sure that all seeds and rng-sources are the same for all runs, i.e., the only thing changing is the seed given to the shortcutters. The curves we show below (and showed previously) first take the median over three shortcutting seeds for each of three initial paths, and then the median over those three path-level curves. The shaded range spans the minimum to maximum of the three path-level medians.

The methods from prior work or from the baselines section we are using are: ‘random robot + compatible run’, ‘random robot subset + compatible run’, ‘WAFR single robot’, ‘Huang round robin w/o prioritization’

The new methods we add are

• Tree + gain: We first run the tree part in order to find the long shortcuts, and then, once the maximum number of splits has been reached, we switch to ‘gain’, where we then sample endpoints uniformly in the selected mode. This is what is currently the standard method in the multi-robot multi-goal planner benchmark.
• Tree + group gain: Same as above, but instead of ‘gain’, we do the ‘group gain’ as fallback after the tree is done.
• Gain only: Instead of running the tree in the beginning, we only run the ‘gain’ selector.
• Our round robin: We combine ‘Full configuration space’, ‘Tree’ and ‘Gain’ into a round-robin meta-approach.

Offline shortcutting

For offline shortcutting, we run a planner, and take the initial path that it produces, and feed this path to all shortcutting algorithms.

Plots with a logarithmic time axis

In the plots above we see that there is no large difference between the methods anymore. ‘Gain’ is not doing great on one of the scenarios, but is relatively similar to the other in the rest. The same is true for ‘random robot + compatible run’, and ‘random robot subset + compatible run’. Tree + group gain, and our own round robin version seems to consistently be among the best.

We have run the best versions from here on a broader set of scenarios to see if there’s any significant difference in some of the other scenarios.To save some time, I have only ran this on one path + 3 seeds. The summary is effectively ‘there is no huge difference in most scenarios’, but you can see the plots below.

Additional offline scenarios

Online shortcutting

For online shortcutting, we simply run the planner in its optimizing mode, which calls the shortcutter whenever a new plan that improves upon the previous cost is found.

The new shortcutted path is then fed back into the planner again, to refine the tree.

Plots with a logarithmic time axis

The results from the offline experiments mostly carry over: ‘Tree + group gain’ is consistently among the best, but the ‘tree + gain’ is very similar.

I want to mention here again that in the current implementation, we do not carry state between the shortcut invocations, since the path is likely changed by continuing running the planner. Thus, it is non-trivial to keep estimates over the modes consistent.

However, this means that we ‘lose’ the progress in the tree-splitting, or the estimates for possible mode-improvements that we collected between shortcutting calls.

How much time should the shortcutters get in the planner? In all the online plots above, we used the default version of how many shortcutting invocations we do. However, in many settings, this results in only ~10% of the total planning time spent shortcutting. To a certain extent, this explains why the online results were relatively similar for all shortcutters: They simply could not actually spend enough time to show where they were better, if the difference was not already quite big.

I was wondering if we should allocate more (or less?) time then.We are only running a single path seed here, but still three shortcutting seeds.

Plots with a logarithmic time axis

But we can see that the differences are not that big. There’s a chance that we should give much more budget, but that would fundamentally change the planner, and I am not a fan of that.

Scaling with the number of robots

I also did some very brief scaling tests: We have a setting where we just duplicate the same single robot stacking scenario \(N\) times. This means that we are getting a single problem with more robots and more tasks, but the robots do not interact with each other at all in this setting. In an ideal world, this would also mean that the achievable cost in all scenarios is exactly the same (or technically ever so slightly higher, as we are imposing an order on the pick and place actions).

We ran the shortcutters there as well, and again as before, we can see similar effects as before. They do seem to get more extreme the more robots we add, but I would argue that this is quite natural, as the problems become more complex.

Take away?

I think with the results that we have seen now, it is hard to point to one specific shortcutting approach that is the one you should be using, as they are all pretty similar:

• When running the shortcutters as part of the planning loop, it does not matter too much what you do as long as you sample the modes directly.
• When running the planners offline as pure post processing round robin seems to be a decent idea.

My personal take away is that the only thing that you really need to be doing is sampling mode-compatible shortcuts directly. The rest does not really matter too much, and thus, the approach I will be implementing in the future as standard version is the simplest one. To me, this is the ‘Tree + group gain’ shortcutter.

Future work

There are some things that I omitted here for now:

• I tested only up to 4 robots. The conclusion might change if we run this on the same scaling study that I did in the WAFR paper.
• I only tested this with Bidirectional RRT as planner. Some conclusions might change for other planners, both for the offline, and for the online settings.

Citation

If you found this work helpful, please reach out and let me know, and cite our multi-robot planning paper, or this blog post, depending on what you found helpful.

@article{hartmann2026shortcutting,
  title = {Shortcutting multi-robot multi-goal plans},
  author = {Valentin N. Hartmann},
  year = {2026},
  month = {October},
  url = {https://vhartman.github.io/multi-robot-shortcutting/}
}