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.

What problem does this paper attempt to address?

The paper is primarily dedicated to addressing the constraint satisfaction problem in robot trajectory optimization, particularly in finding diverse and feasible trajectory solutions under complex constraint conditions. Specifically, the paper proposes the "Constrained Stein Variational Trajectory Optimization" (CSVTO) algorithm. The goal of CSVTO is to solve trajectory optimization problems with equality and inequality constraints and generate a set of trajectories that can satisfy these constraints. Traditional approaches either incorporate constraints as penalty terms into the objective function or use barrier functions to ensure that the trajectory meets the constraints. However, both methods have limitations: the former requires careful tuning of penalty weights to avoid conflicts with the objective function, while the latter is only applicable to inequality constraints and has not yet been applied within the inference framework of trajectory optimization. The key contributions of CSVTO include: 1. **New problem framework**: Reformulating the constrained trajectory optimization problem into a new form—constrained functional minimization over trajectory distributions, which avoids directly incorporating constraints as penalty terms in the objective function. 2. **Extension of the SVGD algorithm**: Proposing a constrained trajectory optimization algorithm based on Stein Variational Gradient Descent (SVGD) that can handle problems with differentiable equality and inequality constraints. 3. **Particle resampling step**: Introducing a novel particle resampling step that resamples and perturbs particles within the tangent space of the constraints, helping to escape local optima. 4. **Experimental validation**: Validating the effectiveness of CSVTO through a series of complex constrained tasks, including a 12-degree-of-freedom (DOF) underactuated quadrotor task and two highly constrained 7-DOF manipulation tasks. Experimental results show that CSVTO outperforms baseline methods in handling challenging, highly constrained tasks, achieving a complete success rate in some tasks and demonstrating the best performance in constraint satisfaction. Additionally, CSVTO exhibits online adaptability, such as adjusting its strategy in real-time to continue the task when a human interferes with the robot's actions.

Constrained Stein Variational Trajectory Optimization

Discrete States-Based Trajectory Planning for Nonholonomic Robots

Improved Sequential Convex Programming Based on Pseudospectral Discretization for Entry Trajectory Optimization

Constrained Sampling-based Trajectory Optimization using Stochastic Approximation

Direct Collocation Methods for Trajectory Optimization in Constrained Robotic Systems

GuSTO: Guaranteed Sequential Trajectory Optimization via Sequential Convex Programming

Trajectory optimization for lunar rover performing vertical takeoff vertical landing maneuvers in the presence of terrain

Variable Grasp Pose and Commitment for Trajectory Optimization

Fast and Certifiable Trajectory Optimization

Second-Order Stein Variational Dynamic Optimization

Successive Convexification for Trajectory Optimization with Continuous-Time Constraint Satisfaction

Low-Thrust Trajectory Design Optimization with Stochastic Convolution

Precise trajectory optimization for articulated wheeled vehicles in cluttered environments

An Incremental Strategy for Tractor-Trailer Vehicle Global Trajectory Optimization in the Presence of Obstacles.

Equality Constrained Diffusion for Direct Trajectory Optimization

Simultaneous dynamic optimization: A trajectory planning method for nonholonomic car-like robots

Simultaneous Trajectory Optimization and Contact Selection for Contact-rich Manipulation with High-Fidelity Geometry

Provably Feasible and Stable White-Box Trajectory Optimization

SGD for robot motion? The effectiveness of stochastic optimization on a new benchmark for biped locomotion tasks

Fast Adaptation of Manipulator Trajectories to Task Perturbation By Differentiating through the Optimal Solution

Constrained Trajectory Optimization for Hybrid Dynamical Systems