With the recent influx in demand for multi-robot systems throughout industry and academia, there is an increasing need for faster, robust, and generalizable path planning algorithms. Similarly, given the inherent connection between control algorithms and multi-robot path planners, there is in turn an increased demand for fast, efficient, and robust controllers. We propose a scalable joint path planning and control algorithm for multi-robot systems with constrained behaviours based on factor graph optimization. We demonstrate our algorithm on a series of hardware and simulated experiments. Our algorithm is consistently able to recover from disturbances and avoid obstacles while outperforming state-of-the-art methods in optimization time, path deviation, and inter-robot errors. The code is open source.

Paper Code



Obstacle Scalability Robot Scalability
Results from scalability experiments. Obstacle scalability experiment highlights how with an increase in environment complexity, our approach is able to scale constantly, and outperform comparable approaches by a significant margin. In robot scalability, we are also consistently able to outperform the state of the art approaches.


  author={Jaafar, Hussein Ali and Kao, Cheng-Hao and Saeedi, Sajad},
  journal={IEEE Control Systems Letters}, 
  title={MR.CAP: Multi-Robot Joint Control and Planning for Object Transport}, 


See the following figures for more details on the algorithm such as Pareto Frontiers and optimization time comparisons.

3D Experiment
3D Experiment
Pareto Frontier for 1 obstacle
Pareto Frontier for 1 obstacle
Pareto Frontier for 2 obstacle
Pareto Frontier for 2 obstacles
Pareto Frontier for 5 obstacle
Pareto Frontier for 5 obstacles
Pareto Frontier for 7 obstacle
Pareto Frontier for 7 obstacles