Continuous Trajectory Optimization in Non-convex Environments: A Local ADMM Solver for Graphs of Convex Sets

Lukas Pries, Jon Arrizabalaga, Zac Manchester, Markus Ryll

Keywords: Trajectory Optimization, Motion Planning, Non-convex Optimization, ADMM, Mixed-Integer Optimization, Graphs of Convex Sets (GCS), Discrete-Continuous Optimization

Abstract: Computing dynamically feasible trajectories in non-convex environments requires reasoning over both continuous motion and discrete spatial structure. In this work, we present a novel numerical solver that couples these two aspects through a customized implementation of the Alternating Direction Method of Multipliers (ADMM). Trajectories are parameterized as polynomials, allowing the primal update to be solved efficiently as a quadratic minimum-control-effort problem. To handle the non-convex geometry of the environment, we introduce a spatio-temporal allocation graph that encodes the assignment of trajectory segments to convex regions. The corresponding slack update reduces to a shortest-path search in this graph, allowing the solver to efficiently explore discrete options while maintaining continuous trajectory consistency within a unified iterative framework. We demonstrate the effectiveness of the approach on several challenging planning scenarios and discuss its potential for scalable trajectory optimization in robotics.

Submission Number: 20