Reliable physical interaction with objects is critical for robots performing contact-rich manipulation tasks such as key insertion, cable connection, fastening, and assembly. In these settings, small estimation errors in relative pose or local geometry can lead to missed contacts, jamming, excessive forces, or task failure. A central difficulty is partial observability — the robot must reason about the pose of a hole or cavity that is only indirectly observed through vision, proprioception, and contact. Depth sensing provides useful global information but suffers from sensor noise, occlusions, restricted viewpoints, and ambiguities due to part geometry. The most task-relevant surfaces may be hidden inside the hole or only revealed through physical interaction.
The key insight is that force–torque sensing becomes spatially informative when interpreted through geometry and simulation. A raw wrench does not directly reveal a contact point or object pose. However, conditioned on the robot state, the probing action, and a hypothesized object pose, a physics simulation can predict what contact evidence should be observed. Comparing simulated and measured contact therefore defines a likelihood over possible poses. In this way, contact is not merely a feedback signal for control, but a source of Bayesian evidence about the hidden scene state. BayesContact also leverages the current belief to choose the next action for probing, turning contact into an active sensing mechanism.
Multi-Sensor Fusion for Contact-Rich Manipulation
Sensor fusion methods integrate diverse information sources into a shared state representation suitable for task execution. In contact-rich manipulation, the sources include cameras, force–torque sensing, and robot proprioception. While these methods are effective for planar pushing tasks, forceful interaction requires contact models that relate interacting object configurations with interaction forces. This work builds upon the Bayesian inference framework used by prior methods but integrates sensor likelihoods provided by simulators — particularly graphics and physics engines — which provide additional modeling flexibility.
Object Pose Estimation Using Contact
Prior works attempt to estimate pose of an object using only contact sensing. Some approaches jointly estimate pose and geometry of an object in a planar pushing task using a probabilistic representation and factor graph optimization, employing limit surface models to capture interaction dynamics with the assumption of known contact location on the robot. These provide effective contact model approximations for planar pushing, but forceful interaction tasks like peg-in-hole insertion require approximations that track contact location, which are available via simulation when conditioned on geometry.
Differentiable contact features have been proposed to use contact models for forceful interaction tasks, demonstrating bi-level optimization with the inner optimization solving for contact forces and the outer optimization solving for object configuration. This method follows a similar approach, using sampling-based inference for object configuration proposals and a simulator for the contact model evaluation. Tactile sensing is a common paradigm to estimate and correct for pose differences, with approaches using tactile sensing modalities and learning contact line estimators for peg-in-hole tasks, or learning tactile sensor models to fuse tactile imagery with visual depth images. Though effective, these approaches make assumptions of known extrinsic contact locations. This work infers contact locations and fuses the resulting evidence to estimate the hidden hole pose.
Sampling-Based Inference and Contact
The general approximate inference frameworks proposed for pose estimation tasks involve fusing information via factor graphs. When integrating various sensing modalities, factor graph-based optimization requires a differentiable forward model to be specified. Due to the inherent discontinuous and non-linear nature of contact, task-specific contact approximations are often used to make them amenable to state inference. This work extends this formulation with geometry-conditioned force likelihoods and depth-based image likelihoods, enabling contact-aware scene understanding for estimating the hole pose in a peg-insertion task.
Simulation-Based Bayesian Inference
Pose estimation is formulated as Bayesian inference over an unknown object pose. Given an observation, Bayesian inference updates a prior belief into a posterior. For sequential observations, this update can be written recursively.
Sequential Monte Carlo
Since the posterior over object pose can be multimodal due to occlusion, symmetry, visual ambiguity, and ambiguous contact explanations, the belief is represented using a weighted set of particles, where each particle is a pose hypothesis with a normalized weight. Sequential Monte Carlo (SMC) updates this belief by proposing pose hypotheses, evaluating their simulation-based likelihoods, normalizing particle weights, and resampling when required. This representation is well suited to BayesContact because each particle can be independently rendered or simulated to evaluate its consistency with visual and contact measurements.
System Overview
For inference, each particle is used to determine depth observations from a graphics renderer, and contact observations under probing actions from a physics simulator. These simulated observations are compared with the measured depth and force–torque data using likelihoods. These determine the front-end of the estimator. The likelihoods are then used to update the belief (via the particles) using SMC inference, which corresponds to the back-end of BayesContact. Inference proceeds in two stages. First, depth observations initialize a vision posterior using SMC with Metropolis–Hastings refinement. Second, the contact phase warm-starts with the vision posterior and updates the belief using contact-rich probing. During this phase, each probing step may provide both force–torque-derived contact evidence and associated depth observations, which are fused through a joint simulation-based likelihood.
Problem Formulation
Measurements are indexed sequentially — the first measurements correspond to the visual perception phase, and the remaining measurements correspond to contact-rich interaction. The object pose is static across measurements, while the belief is updated sequentially as new visual and contact observations are acquired.
Action Space and Guarded Probing
To gather contact measurements, the robot executes contact actions which specify guarded probing motions used to acquire information about the hole pose. In the contact phase, actions are parameterized as planar and yaw offsets. From the commanded offset, the robot executes a guarded vertical dip along the negative axis. The motion is terminated when either a predefined force–torque threshold is exceeded or a maximum displacement is reached. Actions are executed using a Cartesian impedance controller that tracks an action-dependent reference trajectory. The force–torque measurements acquired during the guarded motion are converted into contact evidence and used in the corresponding Bayesian update.
Experiments
To validate BayesContact, three questions are evaluated:
- Does force–torque-induced contact-location information improve pose observability?
- Does a particle-based simulation-inference belief better preserve and resolve geometric ambiguity than parametric alternatives?
- Does this belief enable active sensing?
Metrics
Pose accuracy is evaluated using ADD-S, position error, and orientation error. ADD-S measures geometric alignment while accounting for object symmetries.
Conclusion
This work proposed BayesContact, a simulation-based inference framework that incorporates geometry-conditioned force–torque likelihoods and depth likelihoods with SMC inference to estimate the belief over pose of a hole in a peg-insertion setting. Through simulation and real robot experiments, force–torque-based contact is shown to improve pose observability over depth-only estimates. The resulting belief representation can be used for information gain-based active sensing.
Frequently Asked Questions
How does BayesContact combine vision and touch information? It uses a Sequential Monte Carlo framework where each pose hypothesis (particle) is independently evaluated against both depth images (via graphics rendering) and force–torque measurements (via physics simulation), and the resulting likelihoods are fused to update the belief.
Why is force–torque sensing useful for pose estimation when the hole surface is hidden? While a raw wrench does not directly reveal pose, conditioning on the robot state, probing action, and hypothesized pose allows a physics simulator to predict expected contact, turning force–torque data into Bayesian evidence about the hole's location.
What makes contact-rich pose estimation challenging for traditional factor graph methods? Contact is inherently discontinuous and nonlinear, making it difficult to specify differentiable forward models required by factor graph optimization. BayesContact sidesteps this by using sampling-based inference with simulation-based likelihood evaluation.
Can BayesContact choose its own probing actions? Yes, the belief representation enables active sensing — the robot can select the next probing action (planar and yaw offset) that maximizes expected information gain about the hole pose.