A Benchmark for Optimal Multi-Modal Multi-Robot Multi-Goal Path Planning with Given Robot Assignment

In many industrial robotics applications, multiple robots are working in a shared workspace to complete a set of tasks as quickly as possible. Such settings can be treated as multi-modal multi-robot multi-goal path planning problems, where each robot has to reach an ordered sequence of goals. Existing approaches to this type of problem solve this using prioritization or assume synchronous completion of tasks, and are thus neither optimal nor complete. We formalize this problem as a single path planning problem and introduce a benchmark encompassing a diverse range of problem instances including scenarios with various robots, planning horizons, and collaborative tasks such as handovers.

Along with the benchmark, we adapt an RRT* and a PRM* planner to serve as a baseline for the planning problems. Both planners work in the composite space of all robots and introduce the required changes to work in our setting. Unlike existing approaches, our planner and formulation is not restricted to discretized 2D workspaces, supports a changing environment, and works for heterogeneous robot teams over multiple modes with different constraints, and multiple goals.

This benchmark addresses multi-modal, multi-robot, multi-goal path planning, where multiple robots must reach ordered goal sequences while considering interactions like object handovers. Unlike traditional methods, our approach is not restricted to 2D workspaces, supports dynamic environments, and enables heterogeneous robot teams.

The problem is formalized as a single composite-space planning problem, where each robot follows a sequence of goals while avoiding collisions. Some goals require multi-robot coordination, leading to mode transitions where environmental constraints change dynamically.

The goal sequence can be specified as a fully ordered sequence, or more flexibly as a dependency graph between tasks:

The benchmark includes 21 diverse problem instances, ranging from simple validation cases to complex, high-dimensional settings with up to 5 robots and 74 subgoals. Tasks include rearrangement, stacking, mobile assembly, and cooperative manipulation.

Some examples with their corresponding subgoals can be seen below:

Some of the intermediate configurations we show there are in collision since we simply reset the agent without a goal to its home pose.

Two baseline planners—an RRT*-based planner and a PRM*-based planner—are adapted to handle mode transitions and dependency constraints. Both are probabilistically complete and asymptotically optimal, with locally informed sampling and partial shortcutting for improved efficiency.

Standard informed sampling techniques struggle in high-dimensional, multi-goal problems due to large search spaces. To improve efficiency, we introduce locally informed sampling, which selects sampling regions based on the structure of the current best path. By sampling between intermediate waypoints rather than globally, this method accelerates convergence and enhances planner performance.

Below, we show plans from multiple environments.