Pose Estimation for Contact Manipulation with Manifold Particle Filters

We investigate the problem of estimating the state of an object during manipulation. Contact sensors provide valuable information about the object state during actions which involve persistent contact, e.g. pushing. However, contact sensing is very discriminative by nature, and therefore the set of object states which contact a sensor constitutes a lower-dimensional manifold in the state space of the object. This causes stochastic state estimation methods such as particle filters to perform poorly when contact sensors are used. We propose a new algorithm, the manifold particle filter, which uses dual particles directly sampled from the contact manifold to avoid this problem. The algorithm adapts to the probability of contact by dynamically changing the number of dual particles sampled from the manifold. We compare our algorithm to the particle filter through extensive experiments and we show that our algorithm is both faster and better at estimating the state. Our algorithm’s performance improves with increasing sensor accuracy and the filter’s update rate. We implement the algorithm on a real robot using a force/torque sensor and strain gauges to track the pose of a pushed object.