Single Point of Contact Manipulation of Unknown Objects

Stuart AndersonAdvisor: Reid Simmons

School of Computer ScienceCarnegie Mellon University

The Problem

• Given an object with unknown geometric and dynamic parameters, how can a robot discover these parameters?

• A six degree of freedom fiducial (pose sensor) is affixed to the object.

• We can manipulate the object by applying forces to its surface.

• Applied forces and fiducial observations have white gaussian noise

How can we learn about the world?

• Experimentation– Based on a belief about the state of the world

make a prediction of what will happen when a particular action is taken. Then observe what really happens and incorporate this new information into the belief.

Learning

• Two approaches– Extensions to the Kalman filter– Particle Filter

• What do we learn about the system?– Geometry

• pre-selected set of normals• learn their offsets, define a convex hull

– Dynamics Parameters• Mass, Inertial Tensor, Center of Mass, Coefficient

of Friction

Physics

• The motion of a rigid body is described by the Newton-Euler equations.

• For simplicity, we use the term ‘wrench’ to describe vector composed of a force and a torque.

• We also introduce a generalize mass matrix. This allows us to write

I

maf

f

w

Maw

Physics

• In order to predict the motion of an object, we need to know the forces applied to it.

Mawww gravitysurfacemanip

• If we have some belief about the parameters of the object, then we can explicitly compute the wrench due to the manipulator and to gravity

0

ˆ

)(

zmgw

fCMp

fw

gravity

manipcontact

manip

manip

Physics

• The contact wrench cannot be computed explicitly.

)(ˆ)(

)(ˆ)()(

)(areacontact

xfzxpCMx

xfzxpxw

dxxww

load

load

contact

The pressure at any given point is indeterminate and both the normal force and frictional force depend on it. This severely limits what can be known about the contact wrench

Physics

xxudxxufxwxuxw

xfCMx

xfxw

xfCMx

xfxpxw

xxpfdxxp

normal

friction

friction

friction

friction

normal

0)( ,1)( where)(')()(

)z)(ˆ()(

z)(ˆ)('let

)z)(ˆ()(

z)(ˆ)()(

0)( ,)(areacontact

So the total contact wrench falls within the convex hull of the wrenches produced by applying all pressure at a single point.

Physics

• We need to know more about these convex hulls.– The frictional load is modeled by Coulomb friction.

• The upper bound on the magnitude of the frictional load is linear in the contact pressure

• The direction of the frictional load at a given point is the direction that point is sliding. If the point is not sliding then the load direction is not known.

Thus, when the object is sliding we have

)z)(ˆ()(

z)(ˆ)('

xvCMx

xvxw

Physics• If the motion is a pure translation, then the

direction of motion is constant for all points

fCMx

fxw

)()('

and

frquqqu

f

fr

fqu

fq

fqu

]))(1()([))(1()(

So the set of w’(x) where x is the vertices of the convex hull of the contact region defines the convex hull of possible wrenches, and lies within a two dimensional slice of wrench space.

Physics

• If the motion is a generalized rotation then the direction of the frictional load is no longer linear in position.– The velocity of points on the surface is linear in X

though.

CRx

CRxxv

100

001

010

)(ˆ

Physics

• Since distance is linear in polar coordinates, we apply a change of coordinates to x.

))sin()cos((

)sin()cos(

)sin()cos(

1

)cos(

)sin(

)('

yx

zx

zy

CMCMr

CMrCM

rCMCMw

How do we visualize these equations?

Physics

• Developing an exact description of the convex wrench hull in this case is difficult.

• Instead, we take samples of w’(x) and show bounds on the error in the convex hull constructed from these samples.– Because w’(x) is linear in r we need only sample the

minimal and maximal values of r for a given theta.– The number of samples needed for a given maximum

error bound grows linearly in the radius of a circle centered at the rotation center which encloses the contact hull.

Physics

• Finally, when the object is not moving we take additional samples to model the indeterminacy in the direction of the frictional load.

Particle Filter

• Approximate the posterior distribution using a set of particles.– What’s a particle?

• A description of the system – a state vector

– Lets us work with any system, don’t need to worry about linearization.

– But we have to be careful or dimensionality becomes a problem.

• m(1), I(6), mu(1), cm(3), x(3), v(3),geom(~250)

Particle Filter Approach

• Given a particle b, when we make an observation o

• Then resample the distribution based on the relative likelihood of each particle.

)(

)()|()|(

op

bpbopobp

Observations and Predictions

• We observe accelerations based on the fiducial movement.– Caveat: assume we take observations frequently enough that

the forces applied to the object remain about constant. We can stop looking when bad stuff (impact) happens.

– So we need to predict the acceleration based on state and force applied.

– In general, we can’t get an explicit answer to this question since the contact pressure distribution is undetermined.

– But we can compute bounds on the resulting contact force and torque based on the velocity, geometry, and applied force.

• We also observe the pose of the object, and use that to refine geometric estimates.

Particle Filter

• We can’t use a standard particle filter with our system, since it is not possible to compute p(o|b) exactly.– We let each particle have an upper and lower

bound on its probability, instead of an explicit value.

– But how to normalize the upper bound after an observation?

Particle Filter

Particle Filter

• How do we compute a new upper bound for a given particle?

particlesknormal

normal

ft

normalparticlesk

normal

normalnormal

ft

kfopkp

bfopUU

fpkfopkp

fpbfopUU

normal

normal

),|()(

),|(max

)(),|()(

)(),|(max

min

max

x1

min

max

x1

Particle Filter

• We can solve a simpler case by quadratic programming.

),|(maxonsdistributi pressure

bxop

But we have no insight into finding the true normalized upper bound without resorting to general optimization strategies.

Planning and Behaviors

• Planning has a problem with dimensionality too.– The set of actions is a (potentially

unconnected) path through the set of all forces at all points.

– Behaviors are a way to deal with this• Slide (maintaining current contact face)• Switch contact face (a.k.a roll)

Uncertainty

• Given a set of particles, we can simulate a behavior many times, assuming a different particle to be the ‘truth’ each time.

• We can look at the statistics of these trials to see what effect a given behavior will have on the belief state.– Some behaviors tend to reduce the variance

or entropy of the distribution. Pick these.

The Software

• Dynamics Simulation / Visualization– Human Interface– Planning Interface

• Geometric Reasoning– We can refine the convex hull and find the predicted contact hull.

• Wrench Reasoning– We can compute the probability of an observation within known

and controllable error bounds for all motion cases.• Particle Filter

– Working implementation of the particle filter based on an incorrect assumption about the upper bound update.

• Behaviors– Slide, shift face. Reactive control is a little too good because the

force can be shifted instantaneously.