Multi-Robot Multi-Goal Motion Planning with Stochastic Skills

Conservative plan

Reactive policy

Four robots stack eight blocks. The manipulation skills (pick/place) have a stochastic duration. The reactive policy (right) adapts to the observed skill execution and completes the task sooner than the conservative plan (left).

Abstract

As robots are increasingly deployed in groups and share workspaces to execute real-world tasks, planning their concurrent motions around complex manipulation skills becomes essential. These skills involve continuous physical execution and may exhibit stochastic behavior, resulting in variable execution times and uncertain continuous trajectories. Existing planners either limit execution to single-robot scenarios, rely on open-loop paths, or use post-hoc scheduling that prevents dynamic coordination.

In this paper, we address this gap by integrating stochastic skills into sampling-based multi-robot planning by formulating the problem as a Markov Decision Process (MDP) over a multi-modal composite roadmap. For stochastic skills, solving the MDP yields a reactive policy that allows controllable robots to dynamically adapt their motions in response to other robots’ execution of manipulation skills.

By resolving skill uncertainty directly at planning time, this approach avoids the pessimism of conservative baselines and unlocks robust, dynamic multi-robot coordination.

Informal abstract

A common approach in manipulation planning is to combine (possibly learned) controllers with free-space motion planning. This allows for easy generalization, since we only need to learn a skill for a single robot, but can then rely on the planner to coordinate skill invocation. Often, these skills are stochastic, possibly due to noisy observations, or simply because a skill might fail and retry. In this work, we present three different planners, dealing with increasingly complex settings.

We first introduce deterministic skills in multi-robot, multi-goal planning, showing how to include the skills in prioritized planning, RRTs and PRMs.
We then show how to plan with stochastic skills via conservative planning, i.e., just avoiding the skill-executing robot for all its possible realizations.
Finally, we introduce a reactive planner which computes a policy over the possible modes and skill invocations.

In our formulation, the deterministic setting can be seen as a special case of the reactive planner.

Skill-Aware Formulation

In multi-robot, multi-goal, multi-modal motion planning, we plan through a sequence of modes that describe the different scenes, and the different tasks.

In this work, we extend this description by introducing skill modes, where some of the robots are not controllable, but execute a skill.

\begin{equation*} \label{eq:factorization} q = (q^{\mathrm{skill}}, q^{\mathrm{ctrl}}) \in \mathcal{Q}_{\mathcal{R}^{\mathrm{skill}}} \times \mathcal{Q}_{\mathcal{R}^{\mathrm{ctrl}}}. \end{equation*}

This means that our problem becomes a problem of (1) reaching the goals of the robots, and (2) avoiding the skill-executing robots. We assume a skill is given as

\begin{equation*} \label{eq:skill} q^\text{skill}_{k+1} = f(q^\text{skill}_{k}, \text{env}, \omega_k), \end{equation*}

where $\text{env}$ is the static scene, and $\omega$ is the noise. We assume these skills are given to us, and could be, e.g., a learned 'picking' skill or 'peg-in-hole insertion' skill, but there are effectively no limits as long as the equation above is fulfilled. In this setting, we also assume that the skills avoid collisions with the environment on their own.

When we are considering skills in our planner, we no longer only want a path that is to be followed open-loop — we want to compute a policy that is able to react to the possible scenarios that a skill can invoke. This also means that we are no longer optimizing just the makespan or the path length, but we want to minimize the expected cost:

\begin{equation*} \label{eq:objective} \pi^\star = \arg\min_\pi \mathbb{E}\left[\sum_{k=0}^{T-1} c_{\mathrm{time}}(q_k, q_{k+1})\right], \end{equation*}

where

\begin{equation*} \label{eq:stage-cost} c_{\mathrm{time}}(q_k, q_{k+1}) = \begin{cases} \Delta t & \text{in skill modes,}\\ c(q_k, q_{k+1})/v_{\max} & \text{in non-skill modes.} \end{cases} \end{equation*}

Planning with Skills

Deterministic Skill Planning

With the factorization in the configuration space introduced above, we can easily extend most (sampling-based) motion planning approaches by modifying the steering or the extend functions. We show this in three examples:

In prioritized motion planning, we plan robot tasks one by one. This then means that if we are in a skill mode for the robot we are planning for, we simply roll out the skill.
For planning with an RRT, we largely proceed the same way as for a normal composite space RRT. For steering towards the randomly sampled configuration we then exchange the normal linear interpolation with the factorized version: We do the linear interpolation for the steerable robots, and we roll out the skill for the skill-executing robots.
We proceed similarly in a PRM: We again factorize the space, build a normal roadmap for the steerable robots, and roll out the skills. Here, we do have to be careful with where we introduce edges between the skill vertices.

Stochastic Skill Planning

Conservative Baseline

Compared to the deterministic version above, stochastic planning can then be done by taking the full distribution into account: In principle, we can then also just use the deterministic approach, but with a very inflated collision region (i.e., all rollouts). The slightly less pessimistic version of the conservative planner only avoids collisions with the parts of the rollouts at a specific time slice.

Reactive Policy

Finally, we compute the reactive policy by building a roadmap taking the probability of skill transitions into account, and then compute the value function via backward value iteration.

With that value function, at runtime, we can then just look up the best actions of the controllable robots by maximizing the value, which automatically reacts to possible versions of the skill execution.

Examples

Below, we show how the conservative and the reactive planner deal with the square island, where the skill can decide if the skill-robot (orange) goes up or down. The policy then has to decide how to react to what the skill is doing. The task here is to swap places two times, i.e. to visit the other robots' starting point, and then return home.

We can see below how the reactive planer is able to finish first in both versions of the skill, wheas the conservative planer has to wait for the worst case execution of the skill, and is thus slower overall.

Conservative planner (skill: up)

Reactive planner (skill: up)

Conservative planner (skill: down)

Reactive planner (skill: down)

BibTeX

@inproceedings{schnyder2027stochasticskills,
  title     = {{Multi-Robot Multi-Goal Motion Planning with Stochastic Skills}},
  author    = {Schnyder, William and Hartmann, Valentin N. and Coros, Stelian},
  year      = {2027},
}