Transcript Single Point of Contact Manipulation of Unknown Objects

Single Point of Contact
Manipulation of Unknown Objects
Stuart Anderson
Advisor: Reid Simmons
School of Computer Science
Carnegie Mellon University
Overview
•
•
•
•
•
Problem motivation
Task Description
High level first pass
Detailed pass
Results and Conclusions
Motivation
• There are many applications for the ability
to manipulate in the presence of
uncertainty about the object being
manipulated:
– Mars Rover Rock Scenario
– Possible Nanomanipulation Applications?
– General autonomy
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.
Simple Physics
• Consider a single particle in space.
– The only unknown parameter is mass.
– We represent our uncertainty about the mass
using a probability distribution over potential
masses.
– We push the object with a given force, and
believe we will observe a given acceleration.
– Compute a new uncertainty (or belief)
distribution based on this observation
Simple Physics
• Suppose the object can weigh 1 or 2 kg.
– p(1kg)=.9, p(2kg)=.1.
• We push with 2N of force.
– Suppose we observe an acceleration of 1.
m


p a  1 2 | m  2kg  pm  2kg 
m
s


p m  2kg | a  1 2   
m
s 


p a  1 2 
s 

n(1, obs ) (1)  .1
pm  2kg  
n(1, obs) (1)  .1  n(.5, obs ) (1)  .9
Simple Physics
• Over many observations we expect the
distribution to converge to the true value of
the mass.
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
f  ma
  I
f
w 
 
w  Ma
Physics
• In order to predict the motion of an object, we
need to know the forces applied to it.
wmanip  wsurface  wgravity  Ma
• 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
f manip


wmanip  

(
p

CM
)

f
manip 
 contact
mgzˆ 
wgravity  

0


Physics
• The contact wrench cannot be computed
explicitly.
wcontact 
 w( x)dx
contact area
p( x) zˆ  f load ( x)


w( x)  

ˆ




x

CM

p
(
x
)
z

f
(
x
)
load


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
 p( x)dx  f
Physics
normal
, p( x)  0x
contact area


fˆ friction( x)  ẑ
w( x)  p( x) 

ˆ
( x  CM )  ( f friction( x)  ẑ)


fˆ friction( x)  ẑ
let w' ( x)  

ˆ
( x  CM )  ( f friction( x)  ẑ)
w( x)  u ( x) w' ( x) f normal where  u ( x)dx  1, u ( x)  0x
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
 vˆ( x)  ẑ


w' ( x)  

( x  CM )  ( vˆ( x)  ẑ)
Physics
• If the motion is a pure translation, then the
direction of motion is constant for all points
f

w' ( x)  
( x  CM ) 


f
and
 f
u (q) 
q 

 f
 (1  u (q)) 

f
r 
f
 


f  [u (q)q  (1  u (q)) r ] 

f 
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.
0 1 0

ˆv( x)  1

0
0
0

 x  CR

0
x  CR

1




Physics
• Since distance is linear in polar
coordinates, we apply a change of
coordinates to x.
  sin(  )





cos(

)




1
w' ( )  

 CM y  CM z  cos( )  r sin(  ) 
  CM x  r cos( )  CM z  sin(  ) 


  (r  CM x cos( )  CM y sin(  )) 
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
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.
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
• 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
p(b | o) 
p(o | b) p(b)
p (o)
• Then resample the distribution based on
the relative likelihood of each particle.
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?
– We must compute the upper bound over all possible
distributions.




pmax (o | f normal , b)
U  U t 1 max 

x f normal p
 max (o | f normal , b)   p(k ) pmin (o | f normal , k ) 
k particles\ b


Particle Filter
• We can solve a component of this by quadratic
programming.
pmax (o | b) 
max
pressure distributions
p(o | x, b)
Particle Filter
• The minimum probability is easier
– Try all wrenches on facets of the convex hull,
choose the furthest.
Particle Filter
• But how to optimize?




pmax (o | f normal , b)
U  U t 1 max 

x f normal p
 max (o | f normal , b)   p(k ) pmin (o | f normal , k ) 
k particles\ b


• There are a limited number of minima and
maxima, and we can find them (sort of)
– Root finding on a sum of exponential functions with
known coefficients.
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.
Uncertainty
• But we can only compute bounds on these
statistics.
– If sliding has reward 1 with P=[.2,.8] and
rolling has reward 4 with P=[.1,.6], which is
the correct choice? Is there a correct choice?
Does it work?
• Sort of/not yet!
• Computing p(o|b) works, behaviors work,
but the non-linear optimization step in the
particle filter doesn’t work yet.
• Currently, the computed bounds on
distributions include some distributions
which are not possible given the
observations, and the filter does not
converge.
Will it work?
• I hope!
• But maybe the decision to treat the pressure distribution
as totally unknown is unreasonable.
–
–
–
–
But not unjustified under a rigid body model!
Humans are very compliant
Everything else is sort of compliant
What if we worked on a rubber mat?
• What about trying to model the behavior that the physical
model doesn’t describe?
– Lynch did some work on estimating friction that might be a start
to this. However, it would be very hard to make it work with
changing geometric estimates.
– What else could we try?
Other future work
• In the planner, we should select a small
subset of ‘representative particles’ from
the distribution and only use these for
computing aggregate statistics.
• Rao-Blackwellization in the particle filter
for parameters with well defined effects or
no effects on the w’ hull.
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.