Constrained Stein Variational Trajectory Optimization

TL;DR

CSVTO introduces a novel constrained trajectory optimization method using Stein Variational Gradient Descent, enabling diverse, constraint-satisfying trajectories and outperforming baselines in complex tasks.

Contribution

The paper proposes CSVTO, a new constrained trajectory optimization algorithm that avoids penalty methods and generates diverse solutions using SVGD with a novel re-sampling step.

Findings

CSVTO outperforms baselines in highly-constrained tasks.

It generates diverse trajectories that better satisfy constraints.

Demonstrated success in a 7DoF wrench manipulation task.

Abstract

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.