Search papers, labs, and topics across Lattice.
This paper introduces SteinSQP, a novel constrained Stein variational inference method designed for diverse feasible robot motion planning. By evolving an interacting particle ensemble and embedding constraints into a kernel-space sequential quadratic programming (SQP) subproblem, SteinSQP effectively balances objective value, constraint satisfaction, and particle diversity. The method demonstrates superior performance across five constrained motion-planning tasks, achieving faster convergence and improved feasibility compared to existing approaches.
SteinSQP achieves robust and diverse motion planning by ensuring all sampled trajectories are feasible while significantly speeding up convergence compared to traditional methods.
Robot motion planning is inherently multimodal, yet classical planners typically return only a single solution. Probabilistic formulations address this limitation by maintaining a distribution over motions, allowing the planner to reason over multiple low-cost alternatives. In robotics, however, motion samples must also satisfy strict constraints, including collision avoidance, joint limits, contact conditions, and dynamics consistency. These hard requirements make motion sampling substantially more challenging: within a limited planning budget, the ensemble must cover diverse low-cost motions while ensuring that every sample remains feasible under the relevant constraints. We propose SteinSQP (Stein Variational Sequential Quadratic Programming), a constrained Stein variational inference method for diverse feasible robot motion sampling. SteinSQP evolves an interacting particle ensemble, as in Stein variational methods, while embedding constraints directly into a kernel-space SQP subproblem. We solve the resulting constrained Stein-Newton subproblem with a GPU-friendly matrix-free primal-dual algorithm, enabling efficient batched ensemble updates. To globalize the method, we introduce an ensemble-level merit function that jointly balances objective value, constraint violation, and particle diversity. Across five constrained motion-planning tasks, SteinSQP returns fully feasible ensembles while preserving diverse motion alternatives. Compared with first-order constrained Stein baselines and serial multistart nonlinear programming, SteinSQP shows faster and more robust ensemble convergence in terms of iterations, improves particle-wise feasibility, and achieves faster batched time-to-solution on challenging robot-scale tasks.
