Contact-based optimization for collision-free motion planning for a million robots

This research introduces an algorithm called sequential contact insertion (SCI) for multi-agent robotic systems. By exploiting the structure of optimal trajectories and organizing optimization around contacts where robots reach minimum separation, the method formulates a coupled algebraic system to reconstruct trajectories analytically. The approach starts from unconstrained trajectories, inserts contacts at separation violations, and jointly adjusts their times and states. Numerical experiments demonstrate that SCI can plan for one million robots in minutes and thousands of robots in fractions of a second, achieving median speedups of several orders of magnitude compared to fixed-grid sequential convex programming baselines while maintaining matching solution accuracy.