Stein Variational Ergodic Surface Coverage with SE(3) Constraints

Contact-centric tasks on surfaces, ranging from inspection and cleaning to sanding and polishing, require robots to systematically cover the surface while maintaining stable contact. Ergodic control generates trajectories that spend time at a location proportional to a desired, task-specific spatial distribution, enabling efficient information gathering and coverage. However, traditional ergodic control methods rely on prior knowledge of surface geometry or require a vision sensory input to scan the geometry beforehand, limiting their applicability in real-world scenarios with unknown or dynamic environments. This paper introduces a novel online ergodic control framework that achieves systematic surface coverage while simultaneously reconstructing unknown surface geometry. We employ a Gaussian Process Implicit Surface (GPIS) model that learns global surface geometry from intrinsic tactile sensing during execution. For efficient online planning, we approximate the surface locally using point clouds sampled from tangent planes at observed contact points and iteratively fit them to the Gaussian Process. This approximation simultaneously serves as the sampling domain for both the target and the coverage distributions. We employ a heat-diffusion analogy to compute potential fields that guide ergodic exploration, translating spatial coverage objectives into smooth robot trajectories. We demonstrate our framework through simulation and real-robot experiments, validating simultaneous ergodic coverage and online surface geometry learning with reconstruction errors approaching the ground truth.

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.

Reliable autonomous mapping, environmental sampling, last-mile logistics, and infrastructure deployment depend on the optimal surface area-balanced distribution of Spatial Reference Sites (SRS). Conventional 2D Euclidean methods often fail in high-relief environments by neglecting topographic variations and physical obstructions. This leads to significant planimetric distortion, spatial clustering, and the placement of targets in inaccessible or shadowed regions, compromising both data integrity and operational safety. This paper introduces ToPos, an automated framework for TOPography-aware Optimal Sampling on topographic manifolds. We treat the terrain as a discrete 2-dimensional manifold embedded in 3D Euclidean space and replace standard flat-map distances with non-Euclidean geodesic distances that follow the actual surface geometry. The point distribution is formulated as an optimization problem using a Constrained Geodesic Voronoi Decomposition, solved via a Riemannian Nesterov Accelerated Gradient (NAG) engine. Our approach restricts target locations to a feasible "safe zone," accounting for non-traversable slopes, vegetation, environmental occlusions, etc. Through evaluations on non-convex sinusoidal manifolds, we show that ToPos mitigates planimetric distortion by utilizing geodesic metrics. This approach results in a $\sim$74% improvement in optimal surface area-balanced distribution, as measured by the coefficient of variation (CV) of the Voronoi cell areas. The framework is architected as a Geographic Information System (GIS)-ready micro-service to bolster the mentioned applications. Index Terms: Topographic Manifolds, Geodesic Voronoi Decomposition, Infrastructure Deployment, 3D Mapping, Spatial Sampling, and Non-Euclidean Optimization.