Constrained Stein Variational Trajectory Optimization
This addresses trajectory optimization problems with constraints for robotics and control applications, representing an incremental improvement through a novel combination of existing techniques.
The paper tackles trajectory optimization with constraints by developing CSVTO, which frames it as constrained functional minimization over trajectory distributions and uses Stein Variational Gradient Descent to generate diverse constraint-satisfying trajectories. The method outperforms baselines in highly-constrained tasks like a 7DoF wrench manipulation task, achieving better success and constraint satisfaction.
We present Constrained Stein Variational Trajectory Optimization (CSVTO), an algorithm for performing trajectory optimization with constraints on a set of trajectories in parallel. We frame constrained trajectory optimization as a novel form of constrained functional minimization over trajectory distributions, which avoids treating the constraints as a penalty in the objective and allows us to generate diverse sets of constraint-satisfying trajectories. Our method uses Stein Variational Gradient Descent (SVGD) to find a set of particles that approximates a distribution over low-cost trajectories while obeying constraints. CSVTO is applicable to problems with differentiable equality and inequality constraints and includes a novel particle re-sampling step to escape local minima. By explicitly generating diverse sets of trajectories, CSVTO is better able to avoid poor local minima and is more robust to initialization. We demonstrate that CSVTO outperforms baselines in challenging highly-constrained tasks, such as a 7DoF wrench manipulation task, where CSVTO outperforms all baselines both in success and constraint satisfaction.