Differentiable Particle Optimization for Fast Sequential Manipulation

TL;DR

SPaSM introduces a fully GPU-parallelized framework for sequential robotic manipulation that eliminates CPU-GPU coordination bottlenecks by compiling constraint evaluation, sampling, and gradient-based optimization into end-to-end CUDA kernels. The method solves a two-stage problem: first a parallel particle optimization for object placements (Object Variable CSP), then a joint-space trajectory optimization that lifts placements into robot motions, enabling simultaneous grounding and motion feasibility. Experiments on challenging benchmarks (point-to-point, Tetris packing, and tower stacking) show millisecond-scale solution times with 100% success and up to ~4000× speedups over state-of-the-art baselines, demonstrating real-time replanning capabilities in cluttered environments. The work highlights the importance of integrated grounding and motion optimization and points toward extensions to perception-enabled, receding-horizon, and multi-robot sequential planning.

Abstract

Sequential robot manipulation tasks require finding collision-free trajectories that satisfy geometric constraints across multiple object interactions in potentially high-dimensional configuration spaces. Solving these problems in real-time and at large scales has remained out of reach due to computational requirements. Recently, GPU-based acceleration has shown promising results, but prior methods achieve limited performance due to CPU-GPU data transfer overhead and complex logic that prevents full hardware utilization. To this end, we present SPaSM (Sampling Particle optimization for Sequential Manipulation), a fully GPU-parallelized framework that compiles constraint evaluation, sampling, and gradient-based optimization into optimized CUDA kernels for end-to-end trajectory optimization without CPU coordination. The method consists of a two-stage particle optimization strategy: first solving placement constraints through massively parallel sampling, then lifting solutions to full trajectory optimization in joint space. Unlike hierarchical approaches, SPaSM jointly optimizes object placements and robot trajectories to handle scenarios where motion feasibility constrains placement options. Experimental evaluation on challenging benchmarks demonstrates solution times in the realm of $\textbf{milliseconds}$ with a 100% success rate; a $4000\times$ speedup compared to existing approaches. Code and examples are available at $\href{https://commalab.org/papers/spasm}{commalab.org/papers/spasm}$.

Figures (7)

• Figure 1: Overview of our approach, Sampling Particle optimization for Sequential Manipulation (SPaSM), applied to our 10-block tower problem in clutter. SPaSM jointly optimizes over the robot configuration and object states for sequential pick and place tasks using large scale particle optimization on the GPU, solving end-to-end placements and motions for this problem in 12 milliseconds.
• Figure 2: Cost evolution over 30 optimization steps for the 5-block tetris problem, demonstrating the effectiveness of our optimization strategies. Satisfying states are frequently (though not universally) identified by low initial costs prior to optimization. Pink trajectories represent samples rejected in the initial step (marked by red particles). Rejected samples rarely converge to valid solutions. At step 25, the transition to quadratic costs reveals states with low costs that cannot actually satisfy the constraints.
• Figure 3: Comparison of SPaSM with cuRobo on the MotionBenchMaker dataset. The success rate of both planners is 100%. SPaSM is more than an order of magnitude faster, but as expected, found solution paths are longer.
• Figure 4: Cutaway with an example trajectory generated for an instance of the "cage" problem from MotionBenchMaker chamzas2021motionbenchmaker. SPaSM is able to quickly generate collision free trajectories in complex environments.
• Figure 5: 5-Tetris and 8-Tetris problems. The goal is to pack the blocks into the bounding box. This problem requires