Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Constrained Stein Variational Trajectory Optimization

Thomas Power, Dmitry Berenson

TL;DR

CSVTO addresses constrained trajectory optimization by treating trajectories as a distribution and using Stein Variational Gradient Descent to sample multiple, low-cost trajectories that satisfy differentiable equality and inequality constraints. The method extends Orthogonal-Space SVGD with tangent-space projections, slack-variable handling for inequalities, a Gauss-Newton–style constraint correction, a repulsive kernel for diversity, annealing for exploration, and a resampling mechanism to escape local minima. Empirical results on a 12DoF quadrotor, a 7DoF table-surface task, and a wrench manipulation task show improved constraint satisfaction and robustness to disturbances, outperforming penalty-based baselines and competitive with, or superior to, IPOPT under constrained computation budgets. The work demonstrates the practicality of online replanning with diverse constraint-satisfying trajectory sets and outlines future improvements in differentiability, computation time, and kernel design for real-time deployment.

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.

Constrained Stein Variational Trajectory Optimization

TL;DR

CSVTO addresses constrained trajectory optimization by treating trajectories as a distribution and using Stein Variational Gradient Descent to sample multiple, low-cost trajectories that satisfy differentiable equality and inequality constraints. The method extends Orthogonal-Space SVGD with tangent-space projections, slack-variable handling for inequalities, a Gauss-Newton–style constraint correction, a repulsive kernel for diversity, annealing for exploration, and a resampling mechanism to escape local minima. Empirical results on a 12DoF quadrotor, a 7DoF table-surface task, and a wrench manipulation task show improved constraint satisfaction and robustness to disturbances, outperforming penalty-based baselines and competitive with, or superior to, IPOPT under constrained computation budgets. The work demonstrates the practicality of online replanning with diverse constraint-satisfying trajectory sets and outlines future improvements in differentiability, computation time, and kernel design for real-time deployment.

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.
Paper Structure (39 sections, 1 theorem, 54 equations, 10 figures, 3 tables, 2 algorithms)

This paper contains 39 sections, 1 theorem, 54 equations, 10 figures, 3 tables, 2 algorithms.

Table of Contents

  1. Introduction
  2. Related Work
  3. Trajectory optimization
  4. Sample-based Motion Planning
  5. Planning & Control as Inference
  6. Gradient Flows for constrained optimization
  7. Trajectory Optimization
  8. Variational Inference for Trajectory Optimization
  9. Problem Statement
  10. Stein Variational Gradient Descent
  11. Orthogonal-Space Stein Variational Gradient Descent
  12. Methods
  13. Constrained Stein Trajectory Optimization
  14. Equality constraints
  15. Repulsive term in the tangent space
  16. ...and 24 more sections

Key Result

Theorem 1

Assume that $\nabla h$ is full rank. Let $\phi^* \in \mathcal{H}^d$ be the solution to (eq:stein_problem) with the unconstrained kernel $\mathcal{K}$, and $\phi^*_\perp \in \mathcal{H}^d_\perp$ be the solution to (eq:stein_problem) using the tangent space kernel $\mathcal{K}_\perp$. If the following then the following must be true: where $\mu$ is a vector of Lagrange multipliers.

Figures (10)

  • Figure 1: We use CSVTO to turn a wrench in the real world with online replanning; b) A human disturbs the robot, changing the grasp position of the wrench; c) The robot readjusts the grasp position; d) The robot achieves the desired wrench angle.
  • Figure 2: CSVTO visualized for a 2D problem. The posterior is a mixture of 3 Gaussians, with the log posterior peaks visualized. There is an equality constraint that the particles must lie on the circle. There is also an inequality constraint that the particles must lie outside the shaded region. a) The initial particles are randomly generated and are not necessarily feasible. b) Due to the annealing discussed in section \ref{['sec:methods:annealing']}, early on in the optimization the particles are constraint-satisfying and diverse. c) The particles move towards the relative peaks of the objective, however, the circled particle has become stuck in a poor local minimum due to the constraints, where the gradient of the log posterior is directed towards an infeasible peak. Since the particle is isolated it is not sufficiently affected by the repulsive gradient term that would help escape the local minimum. d) The re-sampling step from section \ref{['sec:methods:resampling']} re-samples the particles, applying noise in the tangent space of the constraints. This eliminates the particle at the poor local minimum. e) The set of particles converges around the local minimum of the objectives while satisfying the constraints.
  • Figure 3: Experimental setup for the quadrotor task. The quadrotor must travel to the goal location, avoiding the obstacle in red while remaining on the blue manifold. The fading yellow shows the path of the obstacle from previous timesteps. a-d) CSVTO maintains a set of trajectories (dashed), with the selected trajectory shown as a solid curve. CSVTO can keep a diverse set of trajectories and switches between them to avoid the moving obstacle. e-f) IPOPT generates an initial trajectory that makes good progress toward the goal and obeys the manifold constraint. However, even after the first timestep the obstacle has moved to render this trajectory infeasible. As the obstacle moves further IPOPT is unable to find an alternative trajectory and ends in a collision.
  • Figure 4: Experimental set-up for the quadrotor with static obstacles task. The quadrotor must travel to the goal location, avoiding the obstacles in red while remaining on the blue manifold.
  • Figure 5: Comparison between CSVTO and IPOPT with multiple initializations on the quadrotor task with static obstacles. We compare CSVTO with 8 trajectory samples vs. 8 runs of IPOPT, both from the same initializations and record the minimum cost achieved from the 8 trajectories over 200 iterations of both. We run 10 trials for each method. The shaded regions show the range of the minimum cost achieved over the 10 trials. We see that from the same initializations, CSVTO finds a solution with a lower cost.
  • ...and 5 more figures

Theorems & Definitions (2)

  • Theorem 1
  • proof