ContactFusion: Stochastic Poisson Surface Maps from Visual and Contact Sensing

TL;DR

This work proposes ContactFusion which combines global mapping with local contact information, fusing point clouds with force sensing, and demonstrates how to fuse contact with visual information into a Stochastic Poisson Surface Map (SPSMap) - a map representation that can be updated with the Stochastic Poisson Surface Reconstruction (SPSR) algorithm.

Abstract

Robust and precise robotic assembly entails insertion of constituent components. Insertion success is hindered when noise in scene understanding exceeds tolerance limits, especially when fabricated with tight tolerances. In this work, we propose ContactFusion which combines global mapping with local contact information, fusing point clouds with force sensing. Our method entails a Rejection Sampling based contact occupancy sensing procedure which estimates contact locations on the end-effector from Force/Torque sensing at the wrist. We demonstrate how to fuse contact with visual information into a Stochastic Poisson Surface Map (SPSMap) - a map representation that can be updated with the Stochastic Poisson Surface Reconstruction (SPSR) algorithm. We first validate the contact occupancy sensor in simulation and show its ability to detect the contact location on the robot from force sensing information. Then, we evaluate our method in a peg-in-hole task, demonstrating an improvement in the hole pose estimate with the fusion of the contact information with the SPSMap.

Figures (7)

• Figure 1: Top: Depiction of an Insertion Hybrid Automata (IHA) in the peg-in-hole task. Bottom: A diagram of our ContactFusion method. First, a point cloud measurement $\mathbf{I}$ is taken of the scene and an initial map $\mathbf{f}^{\text{init}}_{\mathcal{M}}$ is generated. As the robot attempts to insert the peg, contact measurements $\mathbf{z}$ are collected which are then fused into the map $\mathbf{f}_{\mathcal{M}}$.
• Figure 2: An example of the Stochastic Poisson Surface Reconstruction (SPSR). It uses a Gaussian Process to fit an interpolated Normal vector field, and reconstruct the object surface $\mathcal{M}$ by solving a Ordinary Differential Equation. The black arrows show training point cloud locations and normals. Left and Center: Mean of a scalar valued implicit function $\mathbf{f}_{\mathcal{M}}$ and its corresponding variance $\mathbf{K}_{\mathbf{f}}$. This representation allows us to query various statistical quantities around the posterior approximated over the implicit function $\mathbf{f}(x) \sim \mathcal{N}(\mathbf{f}_{\mathcal{M}}, \mathbf{K}_{\mathbf{f}})$. Right: One such useful query is the Occupancy probability $P(\mathbf{x} \in \mathcal{M}) = P(\mathbf{f}_{\mathcal{M}} \leq 0)$. In this work we use SPSR to construct a continuous occupancy map from RGBD and contact information. The surface of the object is defined as the zero level set of the fitted SPSR.
• Figure 3: Qualitative RViz visualisations of our Contact Occupancy Sensor. We utilise a moment arm model combined with friction cone as depicted in \ref{['fig:occ_model']}. Red-Blue denotes decreasing likelihood from 1 to 0.
• Figure 4: Force-Torque residual model used for the Contact Occupancy Sensor. $\mathbf{p}_{O}$ represents sensor origin, $\mathbf{p}_{C}$ represents sample where likelihood is being computed, $\mathbf{f}_{O}$ represents translational element of the wrench.
• Figure 5: Example of SPSMap of a sample rectangular hole from contact point measurements. From Left to Right: the ground truth rectangular mesh file; 1000 contact measurement positions and normals; our SPSMap reconstruction using 50 scanned points; reconstruction using 100 scanned points; full SPSR reconstruction using 1000 scanned points.