Pose Estimation for Contact Manipulation with Manifold Particle Filters · 2013. 5. 19. · Pose Estimation for Contact Manipulation with Manifold Particle Filters ... We address

Pose Estimation for Contact Manipulation with Manifold Particle FiltersMichael C. Koval Mehmet R. Dogar Nancy S. Pollard Siddhartha S. Srinivasa

{mkoval,mdogar,nsp,siddh}@cs.cmu.eduThe Robotics Institute, Carnegie Mellon University

Abstract— We investigate the problem of estimating the stateof an object during manipulation. Contact sensors providevaluable information about the object state during actionswhich involve persistent contact, e.g. pushing. However, contactsensing is very discriminative by nature, and therefore the setof object states which contact a sensor constitutes a lower-dimensional manifold in the state space of the object. Thiscauses stochastic state estimation methods such as particlefilters to perform poorly when contact sensors are used. Wepropose a new algorithm, the manifold particle filter, whichuses dual particles directly sampled from the contact manifoldto avoid this problem. The algorithm adapts to the probabilityof contact by dynamically changing the number of dual particlessampled from the manifold. We compare our algorithm tothe particle filter through extensive experiments and we showthat our algorithm is both faster and better at estimating thestate. Our algorithm’s performance improves with increasingsensor accuracy and the filter’s update rate. We implement thealgorithm on a real robot using a force/torque sensor and straingauges to track the pose of a pushed object.

I. INTRODUCTION

In this paper, we study contact manipulation, where arobot makes persistent contact with the object it is manipu-lating. Imagine reaching into a high cabinet to feel aroundfor the salt shaker, or a robot push-grasping an object intoits hand (Fig. 1-Bottom).

The persistence of contact makes contact sensors, likestrain gauges, force-torque sensors, and tactile pads, a richsource of information for object pose estimation duringmanipulation.

Prior research on pose estimation for contact manipula-tion has focused on using simple analytical motion modelsderived from the physics of pushing to build analytical stateestimators to track the pose of the object from contactpositions on the hand [1].

Unfortunately, there is much uncertainty both in the mo-tion and observation models in the real world: physicalparameters, like friction, mass and pressure distributions arehard to measure and variable, and sensors are noisy. Thisnaturally leads to probabilistic methods, like particle filters,for object pose estimation [2], with stochastic motion andobservation models.

However, we observed that conventional particle filters [2,3] suffer from a startling problem in contact manipulation:they systematically perform worse as sensor resolution orsensor update rate increases.

The problem arises because contact sensing is highlydiscriminative between contact and no-contact states: if aparticle (i.e. a hypothesized object pose) is infinitesimallyclose to the robot hand but not touching it, then contact

xy

θ

Fig. 1. Top: The contact states constitute a lower-dimensional manifoldin the object’s state space. Bottom: Example manipulation of a box withpersistent contact.

sensors will not discriminate between it and another particlewhich is much farther away from the hand. Topologically,the observation space of contact sensors constitute a lowerdimensional manifold in the state space of the pose of theobject (Fig. 1-Top). In practice, particles sampled from thestate space during contact will have very low probability offalling into the observation space which will result in particledeprivation in the vicinity of the correct state. This resultsin particle starvation. Artificially introducing noise into theobservation model sidesteps this problem but comes at theexpense of losing precious information.

We address this problem by deriving the Manifold ParticleFilter (MPF) for state estimation on multiple manifolds ofpossibly different dimensions.

The gist of the algorithm is quite simple. We factorizebelief into the marginal probability of being on a manifoldand the probability of the current state conditioned on thatmanifold. We first sample a manifold. Then, we sample aparticle from that manifold.

Our factorization has two key consequences.First, we can now use a different sampling technique for

each manifold. This allows us to avoid particle starvationon the contact manifold by using the dual proposal distri-bution [4], which samples from the observation model andcomputes importance weights using the motion model.

Second, the marginal adaptively and automatically adjuststhe number of particles on each manifold. So, when thereis no contact, most of the particles are concentrated in theambient space. As soon as contact occurs, the marginal shiftsthe focus onto the contact manifold.

Computing the belief requires the marginal which requiresthe current belief. We subvert this race condition by exploit-ing the discriminative nature of the observation model toapproximate the marginal.

The MPF is not only theoretically sound but also practi-cally useful. We demonstrate:Better state estimation. Through extensive experiments, weshow that the MPF’s state estimate becomes more accurateas we increase the resolution of the contact sensors. Onthe contrary, the regular particle filter becomes less accuratebecause higher-resolution sensors further shrink the numberof particles that agree with an observation.Faster performance. The MPF requires fewer particles andis orders of magnitude faster than the conventional particlefilter. The increase in speed is critical as it enables stateestimation to occur in real-time.Real robot implementation. Finally, we contribute an im-plementation of our algorithm on a real robot, HERB [5],which uses a force-torque sensor and strain gauges to detectcontact on different parts of its hand and estimates the poseof a pushed object.

We also discuss several limitations of our work. Keyamong them is scope. The MPF is designed for persistentcontact, where contact evolves on manifolds. It is unlikely tooutperform a conventional PF when there is very intermittentcontact, like tracking a billiard ball bouncing on a table. TheMPF is also designed to exploit a discriminative observationmodel. It is unlikely to outperform a non-adaptive dual par-ticle filter when the observation model is not discriminative,like a mobile robot outfitted with very accurate LIDAR.

II. RELATED WORK

We borrow the concept of dual particles from mobile robotlocalization literature [4]. Particle filters in this domain sufferfrom a similar particle deprivation problem if a robot usesvery high-accuracy depth rangefinders or cameras. Thesesystems, however, use a mixture proposal distribution with afixed ratio of dual particles to normal particles. This is possi-ble because vision and depth sensors provide high-accuracyreadings independent of the actual state. Conversely, contactsensors provide accurate readings only when the object isin contact with the robot hand. Therefore, normal particlesare necessary for periods of no contact and dual particlesare ideal during contact. Our algorithm achieves this byvarying the number of dual particles according to the contactprobability.

Our focus is on state estimation during contact manipula-tion and particularly during pushing manipulation. Pushingenables robots to perform a wide variety of tasks that are notpossible through pick-and-place manipulation: pushing canmove objects that are too large or heavy to be grasped [6],is effective at manipulating objects under uncertainty [7, 8],

x = (x,y,θ)

(a) State

u

Φx = f (x,u).

(b) Action (c) Observation

Fig. 2. Examples illustrating the (a) state, (b) action, and (c) observationfor the state estimation for contact manipulation problem.

and can be used as pre-grasp manipulation to bring objectsto configurations where they can be easily grasped [9, 10].Additionally, pushing has been used to simultaneously movemultiple objects [11] and singulate an object from a pile [12,13]. Since pushing offers such a dramatic expansion ofmanipulation skills, there have been extensive research onthe fundamental mechanics of pushing [14–17] and on theplanning of pushing operations [17, 18]. Recently, there hasbeen interest in generating push trajectories using samplingbased planners [19, 20] and learning methods [21].

Most of the work described above employs pushing as anopen-loop operation. Conversely, closed-loop actions that usecontact sensors for feedback allows the robot to adapt in real-time and achieve success in more scenarios. One approachof using sensor feedback is to create a feedback controllerthat directly maps sensor readings to actions [22–24]. Thesecontrollers have been shown to be effective for specific tasks,such as locally refining the quality of a grasp [23], but donot generalize to general contact manipulation. Our methodexplicitly estimates the state of the object, which can thenbe used by a higher-level planning algorithm to achieve anarbitrary goal.

Recent work uses probabilistic methods for the tactilelocalization of immovable objects [25–27]. These systemsproduce a number of distinct touch actions that provideinformation about the object pose. In contrast, our systemuses the persistent contact between the hand and the objectduring manipulation.

III. CONVENTIONAL PARTICLE FILTER

In this section, we formalize the problem of state estima-tion during contact manipulation. We introduce the particlefilter as a potential solution and provide insight into why itdegenerates with contact sensors.

A. Pose Estimation for Contact Manipulation

Let x be the state of a dynamical system which evolvesunder actions u and provides observations z. The stateestimation problem addresses the computation of the beliefwhich is a probability distribution over the state space

bel(xt) = p(xt|z1:t, u1:t) (1)

given the past prior actions u1:t and observations z1:t.In our problem, the state is the pose x ∈ SE(2) of the

manipulated object (Fig. 2(a)). Actions are motions of the

hand, given by the velocity twist u ∈ se(2). During contact,the object moves with a velocity fΦ(x, u) where the functionf encodes the physics of the object motion in responseto pushing actions (Fig. 2(b)). The parameter Φ includesenvironmental properties such as the coefficient of frictionbetween the object and the underlying surface, the coefficientof friction between the robot hand and the object, the massdistribution of the object, and the pressure distribution of theobject.

Contact sensors provide observations z about where theobject touches the hand during manipulation. In Fig. 2(c)we illustrate an example distribution of nine contact sensors(shown as bold line segments) on the hand. We assumethat the sensors are not only noisy but also potentially havelow spatial resolution, much like contact sensors in real life.Hence, we assume that a sensor cannot discriminate betweendifferent contact points in its boundary.

B. Bayes Filter

The Bayes filter is the most general algorithm of filteringa belief state given a sequence of actions and observations.The Bayes filter update works recursively:

bel(xt) = η p(zt|xt, ut)∫p(xt|xt−1, ut)bel(xt−1)dxt−1

(2)where η is a normalization factor. p(zt|xt, ut) is called theobservation model and denotes the probability of makinga certain observation given the current state and action.p(xt|xt−1, ut) is called the motion model and denotes theprobability of reaching state xt from state xt−1 with actionut. The Bayes filter update Eq. (2) can be derived fromthe definition of belief Eq. (1) by applying the Bayes’ ruleand assuming that the state is complete, i.e. it satisfiesthe Markov assumption xt ⊥ (u1:t−1, z1:t−1) |xt−1. TheMarkov assumption states that xt is independent of allprevious actions and observations given xt−1.

In our case the observation model p(zt|xt, ut) stronglydiscriminates between states which result in contact and nocontact. This is true by definition for contact sensors such astactile arrays, force/torque sensors, and strain gauges.

We build the motion model by computing fΦ using aquasi-static simulation of pushing [11, 22]. Instead of assum-ing exact values for the parameters Φ, we assume that theyare drawn from a known prior distribution and are stationary.By doing so, we can predict the effect of our actions on thestate using the stochastic motion model p(xt|xt−1, ut).

Since the quasi-static analysis does not allow accelera-tions, our formulation of the state x as the pose—and not thevelocity or acceleration—of the object satisfies the Markovassumption.

The Bayes filter is recursive and, therefore, requires aninitial belief bel(x0). In manipulation, we can initialize thebelief using task-specific knowledge or using other sensors,e.g. cameras.

There are a variety of techniques for representing thebelief state and implementing the Bayes filter. In our case,the motion and observation models are highly non-linear

Algorithm 1 Particle FilterInput: Xt−1, previous particlesOutput: Xt, particles sampled from bel(xt)

1: Xt ← ∅2: for all x[i]

t−1 ∈ Xt−1 do3: Sample x[i]

t ∼ p(xt|x[i]t−1, ut)

4: w[i]t ← p(zt|x[i]

t , ut)

5: Xt ← {x[i]t } ∪Xt

6: Xt ← Resample(Xt)

and lack analytic derivatives. Even worse, the belief stateis non-Gaussian and may be multi-modal. For example, thebelief becomes bimodal in the common case where the fingersweeps through the center of the prior distribution withoutcontacting the object. Together, these challenges preclude usfrom using the Kalman filter or its extended and unscentedvariants. Instead, we can track the belief state using a particlefilter.

C. Particle Filter

The particle filter [3] is a non-parametric formulationof the Bayes filter that represents the belief state with adiscrete set of samples. The samples Xt = {x[i]

t }ni=1 arecalled particles and are distributed according to the beliefstate x[i]

t ∼ bel(xt). When the particle filter receives a newaction and observation, it recursively constructs a new setof particles Xt from the previous set of particles Xt−1 byapplying the motion and observation models.

Algorithm 1 shows an overview of the particle filter. Theparticle filter samples x

[i]t from the proposal distribution

x[i]t ∼

∫p(xt|xt−1, ut)bel(xt−1)dxt−1 (line 3) by forward-

simulating Xt−1 to Xt using the motion model. Next,the algorithm uses the observation model to compute animportance weight w[i]

t = η p(zt|xt, ut) for each forward-simulated particle (line 4). The weighting step assigns higherprobability to particles that are consistent with our currentobservation; i.e. agree with our contact sensors. Finally, thealgorithm resamples the set of weighted particles (line 6) todistribute Xt according to the desired posterior bel(xt).

IV. DEGENERACY OF THE PROPOSAL DISTRIBUTION

The particle filter described above is agnostic to theobservation model and has been applied to a variety ofapplication domains [2, 28]. However, contact sensors areunique because they operate in two discrete states: contactand no contact. During periods of contact, observationsare discriminative and the states for which p(zt|xt, ut) ispeaked form a lower-dimensional manifold around the truestate (Fig. 1). Conversely, during periods of no contact,p(zt|xt, ut) is nearly uniform and provides little usefulinformation. This property makes contact sensors fundamen-tally different than the cameras and depth sensors—sensorswith relatively smooth observation models—typically usedin mobile robot localization [28].

(a)

Upd

ate

Rat

e

Sensor Resolution

(b)

Fig. 3. (a) Illustration of particle deprivation. (b) The swept volumeof contact sensors shrinks as the update rate or resolution of the sensorsincreases.

In practice, particle filters update in discrete steps. Thehand moves a non-zero distance during each step and theswept volume of the contact sensor gains full dimensionality.As such, particle filters do not completely fail at estimatingthe state: instead, they require a large number of particles toincrease the probability that some fall into the small sweptvolume of each sensor. We illustrate this in Fig. 3(a) fora hand pushing a cloud of 500 particles that represent thecenter of a 7 cm bottle. Of the 500 particles (light blue),only 17 (dark orange) contact the hand during a 1 cmstep. This causes particle deprivation around the true stateduring periods of contact: the proposal distribution poorlyapproximates the target distribution and many particles arewasted in low probability regions of the state space.

Surprisingly, this causes the particle filter to perform worseas sensor resolution or the update rate increases. We illus-trate the reason in Fig. 3(b). As sensor resolution increases,the swept volume of each sensor becomes narrower. Asthe update rate increases, the distance traveled by the handbetween updates decreases, and the swept volume becomesshorter. As a result, the particle filter requires a large numberof particles to successfully track the state.

V. MANIFOLD PARTICLE FILTER

We have shown that the conventional particle filter ispoorly suited for contact sensors because the state evolves ona lower-dimensional manifold. In this section, we derive theMPF to solve this problem and show how it can be appliedto contact manipulation.

A. Formulation

Suppose we divide a state space S into m disjoint com-ponents M = {Mi}m1 , where M1, ...,Mm−1 are manifoldsand Mm = S −∪m−1

i=1 Mi is the remaining free space. Thenwe can write the belief in this space as:

bel(xt) =

m∑i

bel(xt|Mi) Pr(xt ∈Mi) (3)

Our algorithm approximates this belief using particles. Foreach particle we first choose which manifold to sample from

Algorithm 2 Manifold Particle FilterInput: Xt−1, previous particlesOutput: Xt, particles sampled from bel(xt)

1: Xt ← ∅2: for i = 1, . . . , |Xt−1| do3: Sample Mi ∼ Pr(xt ∈Mi)4: if Mi 6= Mm then5: Sample x[i]

t ∼p(zt|xt,ut)π(zt|ut)

6: w[i]t = π(zt|ut) · EstimateDensity(Xt−1, x

[i]t )

7: else8: x

[i]t , w

[i]t ← ConventionalSampling(Xt−1, ut, zt)

9: end if10: Xt ← {x[i]

t } ∪Xt

11: Xt ← Resample(Xt)

according to Pr(xt ∈ Mi). Then, we sample the particlefrom that manifold according to bel(xt|Mi).

Ideally, we would compute Pr(xt ∈Mi) as

Pr(xt ∈Mi) =

∫Mi

bel(xt) dxt. (4)

Unfortunately, computing this integral requires knowingbel(xt), which is precisely the distribution that we are tryingto estimate.

Instead, we approximate Pr(xt ∈Mi). Using the previousbelief state to compute

∫Mibel(xt−1) dxt−1 might seem like

a good approximation, but in fact it does not work. To seewhy, consider an update step at which the filter receives anobservation which suggests the state to be on a manifold forthe first time. If we use the previous belief, it will indicatea low probability for the state to be on the manifold andwe will not pick that manifold for sampling. Even if wekeep receiving observations that suggest the manifold, wewill never be able to place particles on it since we will alwaysbe using the belief from the previous step.

Hence we approximate (4) using only the most recentobservation

Pr(xt ∈Mi) ≈∫Mi

p(zt|xt, ut)π(zt|ut)

dxt, (5)

where π(zt|ut) =∫xtp(zt|xt, ut) dxt is the prior probability

of receiving observation zt. This is not, in general, equivalentto Eq. (4). However, Eq. (5) is a good approximation whenp(zt|xt, ut) accurately discriminates between the manifolds.In the limit, when observations perfectly discriminate be-tween manifolds, p(zt|xt, ut) becomes binary and this sam-pling technique introduces no bias. This approximation per-forms well for our purposes since contact sensors accuratelydiscriminate between contact and no-contact.

Finally, we sample a particle x[i]t according to the belief

distribution on the chosen manifold bel(xt|Mi). Our keyinsight is that we can apply a different sampling technique foreach manifold that is specifically designed to take advantageof the structure of Mi. In the case of the free space Mm, wecan sample x[i]

t from S using the conventional technique and

PFM

PF

(a) Prior Belief (b) Before Contact (c) After Contact (d) Final Belief

Fig. 4. Belief state estimated by the PF and MPF for a hand pushing a bottle. (a) p(x0) is a Gaussian prior. (b) Particles that touch the hand areeliminated through a series of “no contact” observations. (c) and (d) As soon as contact is established the belief state collapses to the true state for theMPF.

reject any x[i]t ∈ ∪m−1

i=1 Mi. In the case of a contact manifold,we use dual particles as described in the next section.

B. Manifold Particle Filter for Contact Manipulation

In contact manipulation, there is a single contact manifoldC that contains the set of all states in non-penetrating contactwith the hand. The free space C̄ contains the remainingposes that are not in contact with the hand. Algorithm 2summarizes the application of the manifold particle filter tostate estimation for contact manipulation problem.

If we choose to sample from C, then the conventionalsampling technique will be ineffective. Instead, we impor-tance sample using the dual proposal distribution, whichsamples from the observation model and computes impor-tance weights using the motion model. Formally, we sample

x[i]t ∼

p(zt|xt, ut)π(zt|ut)

(6)

from the observation model (line 5). The correspondingimportance weight

w[i]t = η π(zt|ut)

∫p(xt|xt−1, ut)bel(xt−1)dxt−1 (7)

incorporates the motion model (line 6) and is found bydividing the target distribution Eq. (2) by the proposaldistribution Eq. (6) [4].

The conventional proposal distribution forward-predictsusing the motion model and computes importance weightsusing the observation model. Conversely, the dual proposaldistribution samples particles from the observation model andweights them by how well they agree with the predictionof the motion model. This is the logical inverse of theconventional proposal distribution and has complementarystrengths and weaknesses: the dual distribution performs best

with a discriminative observation model that is tightly peakedaround the true state [4].

Figure 4 shows a comparison of the PF and MPF fora hand pushing a bottle from left-to-right. Each link isequipped with a binary contact sensor and the belief isrendered using kernel density estimation. The PF (Fig. 4-Top) fails to track the state because there are no particlesin the vicinity of the contact observation. The MPF (Fig. 4-Bottom) avoids this problem by sampling dual particles fromthe observation model.

C. Sampling from the Observation Model

One can use different methods to sample states from theobservation model. Since the contact manifold is a two-dimensional manifold embedded in SE(2), we can approx-imate C for an arbitrary object using a relatively small setof pre-computed hand-relative object poses. For each pre-computed sample, we compute its probability using Eq. (6)with the current ut and zt. Then, we sample x[i]

t from thisweighted set.

We can further simplify this computation if we assumeadditional structure in the object model. For example, ifthe object and hand are extruded polygons, then we canavoid pre-computing samples of C by using an analyticrepresentation of the contact manifold.

D. Weighting using the Motion Model

We must compute importance weights for the particles thatwe sampled from the dual proposal distribution. Equation (7)shows that the importance weights consist of three terms:(1) the normalization factor η, (2) the prior probability ofthe observation π(zt|ut), and (3) the probability of the cur-rent state given our history

∫p(xt|xt−1, ut)bel(xt−1)dxt−1.

The constant η appears in both the conventional and dual

PF PF

MPF MPF

Fig. 5. HERB pushing a Pop-Tarts box across the table. The top and bottom rows show the belief as estimated by the PF (top, light blue) and the MPF(bottom, light orange) during different stages of manipulation. The PF and MPF perform similarly when the object contacts the large palm (left), but theMPF outperforms the PF when contact occurs with the small distal links (right).

importance weights and can be omitted. Similarly, π(zt|ut)is the prior probability of receiving observation zt and canbe empirically estimated prior to running the algorithm.

Unfortunately,∫p(xt|xt−1, ut)bel(xt−1)dxt−1 is difficult

to evaluate: we have the ability to sample from this distribu-tion using the motion model, but not evaluate it at an arbitraryvalue of xt. Instead, we estimate the distribution by applyinga density estimation algorithm. First, we forward-simulatethe set of particles Xt−1 to time t with the motion model [4].Next, we use kernel density estimation to approximate∫p(xt|xt−1, ut)bel(xt−1)dxt−1 from the forward-simulated

samples. Kernel density estimation is a natural choice forthis application because it is a non-parametric technique forestimating a probability distribution from a set of samples.

VI. REAL-ROBOT EXPERIMENTS

We evaluated the PF and MPF on HERB, a bimanual mo-bile manipulator designed and built by the Personal RoboticsLaboratory at Carnegie Mellon University [5]. HERB used aBarrett WAM arm equipped with a BarrettHand end-effectorto push a 1.13 kg Pop-Tarts box across the table. We used theBarrettHand’s finger strain gauges to detect binary contact onthe distal links and a wrist-mounted force/torque sensor todetect binary contact on the rest of the hand at approximately10 Hz. We assume that there is a single point contact, so thecombination of those sensors uniquely localizes the point ofcontact in one of nine zones pictured in Fig. 2(c).

Figure 5 shows the two representative runs of the stateestimator on HERB. The pose of the Pop-Tarts box relativeto the hand was tracked by an overhead camera using avisual fiducial and is drawn as a black box. The conventionalparticle filter was run with 500 particles and the MPF wasinitialized with 450 conventional and 50 mixed particles. Asdescribed above, the mixed particles are adaptively sampledusing whichever proposal distribution is best suited for thecurrent state. These parameters were selected such that bothalgorithms took approximately 50 ms to update.

In Fig. 5-Left, the Pop-Tarts box initially contacted thepalm and settled into persistent contact with the hand. Thepalm has a large swept volume, so both the PF (Fig. 5-Top-Left) and MPF (Fig. 5-Bottom-Left) successfully tracked thestate. In both cases, the distribution quickly converged to thetrue state after contact is observed.

In Fig. 5-Right, the Pop-Tarts contacted the hand’s leftdistal link and slips off the finger during the duration of thepush. In this case, the distal link had a small swept volumeand the PF incorrectly converged to the palm. As expected,the MPF successfully traced the state through the duration ofthe contact. This problem would be even more pronouncedif HERB’s observation model included high-resolution data,e.g. tactile arrays.

Please see the accompanying video1 for more examples.

VII. SIMULATION EXPERIMENTS

We have qualitatively shown that the MPF outperforms theparticle filter when used for state estimation on a real robot.In this section, we verify those properties in simulation andshow that these differences are statistically significant.

We will verify the following hypotheses:

H1 The error of the PF and MPF are similar before contact.H2 The MPF has lower error than the PF after contact.H3 Improving resolution increases the error of the PF.H4 Improving resolution reduces the error of the MPF.

Additionally, we demonstrate that the MPF achieves lowererror than the MPF for a fixed amount of processing time.

A. Experiment Design

We implemented both filters in the OpenRAVE [29] sim-ulation environment and evaluated the algorithms with aBarrettHand pushing a bottle. In each simulation, the handmoves in a straight line at a constant speed of 10 cm/s for5 s and receives sensor updates from binary contact sensorsat 10 Hz. The initial pose of the bottle was drawn from aGaussian prior distribution with a randomly chosen mean anda standard deviation of 10 cm. We distributed contact sensorsof a fixed size uniformly across the front surface of thehand. Observations from these sensors were simulated usingfull three-dimensional mesh collision checks between theobject and sensors. For efficiency, the pushing simulation wasimplemented using an approximate, cached collision checkerwith a 1 mm resolution. Sensing errors were simulated byrandomly perturbing the observation with a 5% probability.

1Video is available online at http://youtu.be/iCfApA39PiU.

−3 −2 −1 0 1 2 3 4

Time Since Contact (s)

0

5

10

15

20

25R

MS

E(c

m)

PFMPF

(a) RMSE

−3 −2 −1 0 1 2 3 4

Time Since Contact (s)

0

20

40

60

80

100

Su

cces

sR

ate

(%)

(b) Success Rate

01020304050

Sensor Size (mm)

0

2

4

6

8

10

12

Conta

ctR

MS

E(c

m)

(c) Sensor Resolution

0 10 20 30 40 50 60

Update Rate (Hz)

012345678

Conta

ctR

MS

E(c

m)

(d) Update Rate

Fig. 6. (a) and (b) RMSE and SR of the PF and MPF plotted over time for 500 particles. Contact occurs at t = 0 and is denoted by the dashed line.(c) Effect of sensor resolution on RMSE. The MPF monotonically decreases in RMSE as resolution increases. Conversely, the accuracy of the PF degradeswith high resolution sensors. (d) Real-time performance of the filters. The MPF achieves lower RMSE than the PF given a fixed computational resources.Additionally, the MPF achieves acceptable error with real-time update rates of 60+ Hz. The gray shaded region in (a) denotes the 95% confidence interval.This is omitted from (b), (c), and (d) because the confidence intervals are of a negligible size.

B. Metrics

We will compare the performance of the algorithms withthe following metrics:• Root-mean-square error (RMSE) between the particles

and the true state.• Success rate (SR), the percent of time during which

mean of the particles is within 2 cm of the true state.• Update rate (UR), the maximum rate at which particle

filter updates step can be executed.These three metrics capture different properties of the filters.RMSE is the traditional error metric used in the localizationliterature [4, 28] and measures how closely the particlestrack the true state. Similarly, SR acts as a proxy of howwell the filter would perform while performing a task thatsucceeds under bounded uncertainty. UR directly measuresthe computational complexity and real-time performance ofeach filter.

C. Localization Error

We tracked the RMSE (Fig. 6(a)) and SR (Fig. 6(b)) ofthe two filters over time using a hand with 1 cm resolutioncontact sensors. The x-axes of the plots are aligned suchthat contact occurs at t = 0. Before contact, both filtersbehave similarly and there is a negligible difference in RMSE(3.67 mm < 5 mm, t(24192) = 4.3050, p < .0001). Aftercontact, the MPF achieves 7 cm less RMSE than the PF(t(35556) = 74.86, p < .0001). These results confirmhypotheses H1 and H2: the MPF achieves significantly lowererror than the PF. We present example simulated runs of MPFand PF in Fig. 4.

D. Sensor Resolution

We additionally evaluated the effect of sensor resolutionon the RMSE error of both filters. Figure 6(c) shows theaverage RMSE error during contact as the sensor resolutionis varied from cell sizes of 2 mm to 5 cm. Smaller sizescorrespond to a higher sensing resolution and, ideally, lowererror. As expected, the MPF outperforms the PF at all sensorresolutions.

In both cases, an ANOVA showed that sensor resolutionwas a significant main effect for both the particle filter(F (4, 134674) = 146.462, p < .0001) and the MPF(F (4, 134674) = 137.211, p < .0001) with a negativecorrelation for the PF and a positive correlation for the MPF.In both cases, all but one pairwise tests of sensor resolutionswere significant. This confirms hypotheses H3 and H4.Unlike the MPF, the performance of the PF degrades as thesensor resolution increases. As described in Section IV, thisoccurs because it becomes progressively less likely to sampleparticle with high p(zt|xt, ut) during periods of contact. TheMPF does not suffer from this problem and improves withsensor resolution.

E. Realtime Performance

These filters are intended to be used for real-time feed-back. Therefore, it is important that the filters achieve accept-able error at real-time update rates. Figure 6(d) shows RMSEduring contact as a function of update rate. The update ratewas indirectly manipulated by varying the number of parti-cles from 50 to 10,000 and measuring the time require foreach filter to execute a single update step. All measurementswere taken using a single core of a 2.53 GHz Intel Xeonprocessor.

The MPF achieves acceptable accuracy (e.g. < 2 cmRMSE) with several hundred particles and URs of 60+ Hz.Conversely, the PF only achieves comparable accuracy whenrun with 10,000 particles and has an UR of approximately1 Hz. These results confirm that PF requires a huge numberof particles to accurately track the state and is ill-suited forreal-time use. Conversely, the MPF is fast enough to be usedas real-time feedback.

VIII. DISCUSSION

A. Limitations

We made several simplifying assumptions to find a real-time solution to the state estimation problem during contactmanipulation.

First, we implicitly assume that the hand can only contactthe object that we are manipulating. This may not be possible

in highly cluttered environments where we must contactmultiple objects to achieve the desired task [11]. In futurework, we hope to explore methods of generalizing the MPFto environments with multiple—both static and movable—objects. We believe it is possible to do so by factoringthe belief state (e.g. through Rao-Blackwellization) to avoidrequiring exponentially more particles.

Second, we assume xt ∈ SE(2). This is sufficient forplanar manipulation, but the state space becomes larger ifobjects are articulated, can topple, or can roll. In particular,sampling x[i]

t according to Eq. (6) may become challenging.For most observation models, this is done by representing Cwith a finite set of samples. However, the number of samplesrequired to densely represent C grows exponentially withthe dimension of the state space. We plan to address this byreplacing the importance sampling step with a more efficientMarkov chain Monte Carlo sampling technique.

Third, we can improve our algorithm by recognizingthat tracking the state on different manifolds may havevery different computational costs. They are different duringpushing: tracking particles on the contact manifold requiresmaking a physics-based motion prediction, whereas trackingparticles in the free space is almost free since those particlesdo not move. Similarly, different manifolds may require dif-ferent number of particles. For example, fewer particles maybe enough to track the state on lower dimensional manifolds.The performance of our algorithm can be improved by takinginto account such manifold characteristics.

Finally, in future work, we plan to use the state estimateprovided by the MPF as real-time feedback for physics-basedactions. For example, this could include a closed-loop pushgrasp that uses constant feedback to react in real-time.

B. Contributions and Implications

Contact sensors provide useful information during objectmanipulation. We believe that the state estimation techniquedescribed in this paper could be used to create robust closed-loop actions that use real-time contact feedback to deal withhigh amounts of uncertainty.

In this paper, we have shown that contact sensors funda-mentally differ from the vision and depth sensors tradition-ally used in state estimation (Section IV). Using this insight,we formulated the MPF (Section V) and demonstrated animplementation on a real robot (Section VI) using a simplesensor model. We evaluated the same implementation in asuite of simulations (Section VII) and showed that the MPFoutperforms the conventional particle filter in both accuracyand speed.

REFERENCES[1] Y. bin Jia and M. Erdmann, “Pose and motion from contact,” IJRR,

1999.

[2] L. Zhang and J. C. Trinkle, “The application of particle filtering tograsping acquisition with visual occlusion and tactile sensing,” in IEEEICRA, 2012.

[3] S. Thrun, W. Burgard, and D. Fox, Probabilistic robotics. MIT Press,2005.

[4] S. Thrun, D. Fox, and W. Burgard, “Monte Carlo localization withmixture proposal distribution,” in AAAI, 2000.

[5] S. Srinivasa, D. Berenson, M. Cakmak, A. Collet, M. Dogar, A. Dra-gan, R. Knepper, T. Niemueller, K. Strabala, and M. Vande Weghe,“HERB 2.0: Lessons learned from developing a mobile manipulatorfor the home,” Proceedings of the IEEE, vol. 100, no. 8, pp. 1–19,2012.

[6] M. Dogar and S. Srinivasa, “A framework for push-grasping in clutter,”in RSS, 2011.

[7] R. C. Brost, “Automatic grasp planning in the presence of uncertainty,”IJRR, vol. 7, no. 1, pp. 3–17, 1988.

[8] M. Dogar and S. Srinivasa, “Push-grasping with dexterous hands:Mechanics and a method,” in IEEE/RSJ IROS, 2010.

[9] L. Chang, S. Srinivasa, and N. Pollard, “Planning pre-grasp manipu-lation for transport tasks,” in IEEE ICRA, 2010.

[10] D. Kappler, L. Y. Chang, M. Przybylski, N. Pollard, T. Asfour,and R. Dillmann, “Representation of Pre-Grasp Strategies for ObjectManipulation,” in IEEE-RAS Humanoids, December 2010.

[11] M. Dogar, K. Hsiao, M. Ciocarlie, and S. Srinivasa, “Physics-basedgrasp planning through clutter,” in RSS, 2012.

[12] D. F. Lillian Chang, Joshua R. Smith, “Interactive singulation ofobjects from a pile,” in IROS 2011 PR2 Workshop, 2011.

[13] M. Gupta and G. S. Sukhatme, “Using manipulation primitives forbrick sorting in clutter,” in IEEE ICRA. IEEE, 2012, pp. 3883–3889.

[14] M. T. Mason, “Mechanics and Planning of Manipulator PushingOperations,” IJRR, vol. 5, no. 3, pp. 53–71, 1986.

[15] K. Lynch and M. T. Mason, “Pulling by pushing, slip with infinitefriction, and perfectly rough surfaces,” in IJRR, vol. 14, no. 1.Cambridge, MA: MIT Press Journals, April 1995, pp. 174–183.

[16] R. D. Howe and M. R. Cutkosky, “Practical Force-Motion Models forSliding Manipulation,” IJRR, vol. 15, no. 6, pp. 557–572, 1996.

[17] K. M. Lynch and M. T. Mason, “Stable Pushing: Mechanics, Control-lability, and Planning,” IJRR, vol. 15, no. 6, pp. 533–556, 1996.

[18] S. Akella and M. T. Mason, “Posing polygonal objects in the planeby pushing,” IJRR, vol. 17, no. 1, pp. 70–88, January 1998.

[19] M. Lau, J. Mitani, and T. Igarashi, “Automatic learning of pushingstrategy for delivery of irregular-shaped objects,” in IEEE ICRA, 2011.

[20] A. Cosgun, T. Hermans, V. Emeli, and M. Stilman, “Push planningfor object placement on cluttered table surfaces,” in IEEE/RSJ IROS,2011.

[21] C. Zito, Stolkin, M. R. Kopicki, and J. L. Wyatt, “Two-level RRTplanning for robotic push manipulation,” in IEEE/RSJ IROS, 2012.

[22] K. M. Lynch, H. Maekawa, and K. Tanie, “Manipulation and activesensing by pushing using tactile feedback,” in IEEE/RSJ IROS, 1992.

[23] R. Platt, A. H. Fagg, and R. A. Grupen, “Null-space grasp control:theory and experiments,” IEEE Trans. on Robotics, 2010.

[24] P. Pastor, L. Righetti, M. Kalakrishnan, and S. Schaal, “Online move-ment adaptation based on previous sensor experiences,” in IEEE/RSJIROS, 2011, pp. 365–371.

[25] K. Hsiao, “Relatively robust grasping,” Ph.D. dissertation, Mas-sachusetts Institute of Technology, 2009.

[26] S. Javdani, M. Klingensmith, J. A. Bagnell, N. S. Pollard, and S. S.Srinivasa, “Efficient touch based localization through submodularity,”in IEEE ICRA, 2013.

[27] A. Petrovskaya and O. Khatib, “Global localization of objects viatouch,” IEEE Trans. on Robotics, 2011.

[28] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM2.0: An improved particle filtering algorithm for simultaneous local-ization and mapping that provably converges,” in IJCAI, 2003.

[29] R. Diankov and J. Kuffner, “OpenRAVE: A planning architecture forautonomous robotics,” Robotics Institute, Pittsburgh, PA, Tech. Rep.CMU-RI-TR-08-34, 2008.