Stein Variational Ergodic Surface Coverage with SE(3) Constraints

Jiayun Li, Yufeng Jin, Sangli Teng, Dejian Gong, Georgia Chalvatzaki

TL;DR

This paper introduces TSVEC (Task space Stein Variational Ergodic Coverage), a novel framework that enables robots to perform complex surface coverage tasks by combining ergodic trajectory optimization with Stein Variational Gradient Descent (SVGD). By deriving manifold-aware updates and a Gauss-Newton preconditioner specifically for the SE(3) manifold, the authors allow robots to navigate highly non-convex optimization landscapes and achieve precise end-effector control on 3D surfaces.

What Problem Does This Paper Address?

Robotic tasks like sanding, painting, or surgical debridement require "ergodic coverage"—the robot must visit regions of a surface with a frequency proportional to that region's importance (e.g., spending more time where there is more rust to sand).

However, doing this on complex 3D surfaces presents three major hurdles:

  1. Local Minima: Point-cloud targets create "bumpy" optimization landscapes. Standard solvers often get stuck in poor local solutions, failing to cover distant regions of interest.
  2. Geometric Constraints: Robots operate in SE(3) space (3D position + 3D orientation). Most existing "Sampling-as-Optimization" (SAO) methods struggle to respect the intrinsic geometry of this manifold, leading to imprecise poses.
  3. Ill-Conditioning: Long-horizon trajectories (hundreds of steps) make the math behind the optimization very unstable and slow to converge.

Methods

The authors propose TSVEC, which treats trajectory optimization as a particle-based inference problem. Their approach features three main technical innovations:

  • SE(3) SVGD: They derived a version of Stein Variational Gradient Descent that works directly on the SE(3) manifold. A critical component here is Parallel Transport: to aggregate gradient information from different "particles" (candidate trajectories) on a curved manifold, those gradients must be mathematically "transported" to a common reference frame to be combined correctly.
  • Gauss-Newton Preconditioning: Ergodic objectives are essentially non-linear least-squares problems. The authors introduced a Gauss-Newton preconditioner into the SVGD framework, which significantly accelerates convergence for long trajectories that would otherwise be computationally intractable.
  • Manifold-Aware Sampling: Instead of optimizing a single path, TSVEC evolves a population of trajectories. This "diversity" allows the system to explore multiple potential solutions simultaneously, making it much more likely to find a global optimum rather than getting trapped in a local one.
  • Neural SDFs: To handle discrete point clouds smoothly, they use a Neural Signed Distance Function (SDF) to represent object geometry, allowing for efficient gradient-based calculations.

Datasets

The researchers evaluated their method on:

  • 3D Surface Benchmark: A set of six complex meshes (Mustard Bottle, Pig, Spot the cow, Bunny, Hand, and Torus) with specific "Regions of Interest" (ROIs) marked for coverage.
  • Real-World Task: A Franka Emika Panda robot was tasked with drawing the letters "ICRA" and a heart symbol on the curved surface of a cylindrical cooking pot.

Key Insights and Ideas

  • Parallel Transport is the "Secret Sauce": You cannot simply average gradients on a manifold like SE(3) because the "direction" of a gradient depends on where you are standing. Parallel transport allows particles to "talk" to each other and share information about the landscape without violating geometric constraints.
  • Ergodicity as Distribution Matching: By framing coverage as matching a target spatial distribution, the robot naturally prioritizes important areas while still ensuring the entire surface is explored.
  • Diversity Prevents Failure: Traditional planners (like GN or IPOPT) are "greedy" and often collapse into the nearest local minimum. By using SVGD, the trajectories act like a team of explorers that push each other away (via a repulsive kernel) to cover more ground.

Key Results

  • Superior Coverage: In all six benchmark scenarios, TSVEC achieved significantly lower objective values (better coverage) than strong baselines like IPOPT, L-BFGS, and standard Gauss-Newton planners.
  • Recognizable Results: In the real-world drawing task, the standard GN planner produced illegible scribbles because it got stuck in local minima. TSVEC produced clear, recognizable characters on the curved surface.
  • Robustness: TSVEC was shown to be much less sensitive to initial conditions compared to deterministic optimizers.

Limitations and Future Work

  • Computational Overhead: Running 100+ trajectory particles in parallel is more computationally demanding than optimizing a single path, though the authors mitigated this using JAX and GPU acceleration.
  • Hyperparameter Sensitivity: The performance relies on choosing the right kernel length scales and energy weights, which currently require some manual tuning.
  • Strict Constraints: While the method handles SE(3) geometry well, the authors noted that incorporating an Augmented Lagrangian approach could further improve strict compliance with safety or kinematic constraints.
  • Dynamics: The current work focuses on task-space trajectories; integrating full robot dynamics (forces and torques) is a logical next step for high-speed manipulation.

Summary updated

Abstract

Surface manipulation tasks require robots to generate trajectories that comprehensively cover complex 3D surfaces while maintaining precise end-effector poses. Existing ergodic trajectory optimization (TO) methods demonstrate success in coverage tasks, while struggling with point-cloud targets due to the nonconvex optimization landscapes and the inadequate handling of SE(3) constraints in sampling-as-optimization (SAO) techniques. In this work, we introduce a preconditioned SE(3) Stein Variational Gradient Descent (SVGD) approach for SAO ergodic trajectory generation. Our proposed approach comprises multiple innovations. First, we reformulate point-cloud ergodic coverage as a manifold-aware sampling problem. Second, we derive SE(3)-specific SVGD particle updates, and, third, we develop a preconditioner to accelerate TO convergence. Our sampling-based framework consistently identifies superior local optima compared to strong optimization-based and SAO baselines while preserving the SE(3) geometric structure. Experiments on a 3D point-cloud surface coverage benchmark and robotic surface drawing tasks demonstrate that our method achieves superior coverage quality with tractable computation in our setting relative to existing TO and SAO approaches, and is validated in real-world robot experiments.
BibTeX
@misc{li2026stein,
  author = {Jiayun Li and Yufeng Jin and Sangli Teng and Dejian Gong and Georgia Chalvatzaki},
  title = {{Stein Variational Ergodic Surface Coverage with SE(3) Constraints}},
  year = {2026},
  eprint = {2603.09458},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2603.09458},
}

Loading related content...

Figure