Multiplier-Augmented Geometric Motion Algorithm
ROAHM Lab, University of Michigan
Trajectory optimization for robots, from autonomous vehicles to manipulators and legged systems, demands four things at once: speed for near real-time planning, scalability to high-dimensional state spaces, support for nonlinear dynamics and task constraints, and reliable convergence from poor initial guesses. Existing methods struggle to satisfy all four simultaneously. HJB-based dynamic programming is intractable beyond moderate dimensions. DDP variants are fragile far from the optimum. Collocation-based nonlinear programs are slow and sensitive to discretization.
The Affine Geometric Heat Flow (AGHF) reformulates trajectory optimization as a PDE that evolves an infeasible trajectory toward dynamic feasibility, scaling gracefully to high-dimensional systems. But prior AGHF solvers lean on penalty and barrier formulations to enforce constraints, which forces hand-tuning of weights, brittle behavior when cost and feasibility compete, and sensitivity to the initial guess.
We introduce MAGMA (Multiplier-Augmented Geometric Motion Algorithm), an AGHF-based trajectory optimizer built on an augmented Lagrangian structure solved via a primal-dual geometric flow. By co-evolving primal and dual variables, MAGMA automatically balances optimality and constraint satisfaction. This eliminates manual weight tuning, handles general nonlinear constraints and arbitrary cost functions, and remains robust to poor initialization. We additionally develop an infinite-dimensional primal-dual gradient flow model that provides a theoretical foundation for constrained AGHF methods, and demonstrate MAGMA on high-dimensional manipulation tasks against state-of-the-art solvers.
The dual flow automatically accomodates constraint violations, resulting in a fundamentally different optimization landscape.
Barrier methods can be give rise to numerically large values in the gradient for bad initial guesses, MAGMA circumvents that completely!
MAGMA retains and betters the solution quality of previous AGHF formulations, while simultaneously being better than classical DDP methods
Handles general nonlinear constraints and arbitrary cost functions, going beyond previous AGHF formulations.
MAGMA extends the AGHF equation to handle constraints by coupling it with an augmented Lagrangian structure, co-evolving primal trajectories alongside dual multipliers. We characterize this flow theoretically, proving that MAGMA is a gradient flow on an appropriately defined energy.
The numerical implementation utilizes a pseudospectral method-of-lines approach. To handle constraints dynamically, it approximates constraint derivatives using a smooth activation function.
MAGMA was validated across a variety of challenging trajectory generation problems featuring obstacles, state limits, and actuation limits, and evaluated against state-of-the-art solvers.
MAGMA was tested in practical, task-oriented scenarios involving cuboid obstacles, such as reaching into shelves and placing items in confined bins, reflecting real-world industrial and domestic deployments.
Below are visualizations of MAGMA solving these scenarios. Orange represents the initial guess, white is the final optimized solution, and red denotes obstacles.
@article{magma2026,
title = {MAGMA: Multiplier-Augmented Geometric Motion Algorithm},
author = {Ramos Chuquiure, C{\'e}sar E. and Thakur, Vansh
and Enninful Adu, Challen and Vasudevan, Ram},
year = {2026}
}This work was supported by AFOSR MURI FA9550-23-1-0400.