Accurate Pose Estimation Using Contact Manifold Sampling for Safe Peg-in-Hole Insertion of Complex Geometries

TL;DR

This paper introduces a contact-based pose estimation method for robotic peg-in-hole assembly of complex geometries, significantly improving success rates and safety by accurately estimating the pose using contact manifold sampling.

Contribution

The proposed framework uniquely constructs an online contact manifold for precise pose estimation using only contact states, enabling safer and more efficient assembly.

Findings

Achieved 96.7% success rate on complex geometries

Reduced insertion forces and times significantly

Improved safety by minimizing jamming and damage risk

Abstract

Robotic assembly of complex, non-convex geometries with tight clearances remains a challenging problem, demanding precise state estimation for successful insertion. In this work, we propose a novel framework that relies solely on contact states to estimate the full SE(3) pose of a peg relative to a hole. Our method constructs an online submanifold of contact states through primitive motions with just 6 seconds of online execution, subsequently mapping it to an offline contact manifold for precise pose estimation. We demonstrate that without such state estimation, robots risk jamming and excessive force application, potentially causing damage. We evaluate our approach on five industrially relevant, complex geometries with 0.1 to 1.0 mm clearances, achieving a 96.7% success rate - a 6x improvement over primitive-based insertion without state estimation. Additionally, we analyze insertion forces, and overall insertion times, showing our method significantly reduces the average wrench, enabling safer and more efficient assembly.