Learning the Contact Manifold for Accurate Pose Estimation During Peg-in-Hole Insertion of Complex Geometries

TL;DR

This work tackles precise peg-in-hole insertion for complex geometries by blending model-based structure with learning to handle contact dynamics. It builds a reference contact manifold $\mathcal{M}$ offline and uses a lightweight metric projection $\mathcal{F}$ to map observed poses to $\mathcal{M}$, enabling fast, interpretable pose estimation from contact observations $\mathcal{O}_{contact}$. Online, an ICP-like registration aligns the local contact submanifold with the global manifold, achieving sub-mm and sub-degree accuracy and yielding large gains in insertion success (up to $93.3\%$) and computational efficiency (up to $95\times$ faster NN projection). The approach is data-efficient, hardware-agnostic, and particularly suited for tight-tolerance assembly, with potential extensions to informed sampling and simulation-driven manifold generation.

Abstract

Contact-rich assembly of complex, non-convex parts with tight tolerances remains a formidable challenge. Purely model-based methods struggle with discontinuous contact dynamics, while model-free methods require vast data and often lack precision. In this work, we introduce a hybrid framework that uses only contact-state information between a complex peg and its mating hole to recover the full SE(3) pose during assembly. In under 10 seconds of online execution, a sequence of primitive probing motions constructs a local contact submanifold, which is then aligned to a precomputed offline contact manifold to yield sub-mm and sub-degree pose estimates. To eliminate costly k-NN searches, we train a lightweight network that projects sparse contact observations onto the contact manifold and is 95x faster and 18% more accurate. Our method, evaluated on three industrially relevant geometries with clearances of 0.1-1.0 mm, achieves a success rate of 93.3%, a 4.1x improvement compared to primitive-only strategies without state estimation.

Figures (1)

• Figure 1: The overview of our methodology. During the offline phase, a sampling controller is used to sample contact poses and train a model, $\mathcal{F}$, to project neighboring points to the contact manifold. During the online phase, the system is provided an initial hole pose estimate with significant error. The search motion primitive uses this estimate to achieve a partial insertion state. Next, contact pose observation collection is performed by perturbing the peg pose, resulting in the observed contact submanifold, $\mathcal{O}_{contact}$. The contact manifold registration algorithm uses $\mathcal{O}_{contact}$, $\mathcal{F}$, and the initial hole pose estimate to compute a precise pose of the hole with respect to the robot. The system then uses this pose to align the peg and finally insert to achieve the full insertion state. $\mathcal{M}$ represents the contact manifold dataset, $NN$ represents nearest neighbor search. and $\boldsymbol{\otimes}$ represents composing SE(3) poses, corresponding to the multiplication of their homogeneous transformation matrices.