Accepted by the International Journal of Robotics Research.
Pose and Motion from Contact
Yan-Bin Jia Michael Erdmann
The Rob otics Institute
Scho ol of Computer Science
Carnegie Mellon University
Pittsburgh, PA 15213-3890
June 26, 1998
Abstract
In the absence of vision, grasping an object often relies on tactile feedback from
the ngertips. As the nger pushes the object, the ngertip can feel the contact point
move. If the object is known in advance, from this motion the nger may infer the
location of the contact point on the object and thereby the object pose. This paper
primarily investigates the problem of determining the pose orientation and position
and motion velocity and angular velocity of a planar object with known geometry from
such contact motion generated by pushing.
A dynamic analysis of pushing yields a nonlinear system that relates through contact
the object pose and motion to the nger motion. The contact motion on the ngertip
thus encodes certain information about the object pose. Nonlinear observability theory is
employed to show that such information is sucient for the nger to \observe" not only
the pose but also the motion of the object. Therefore a sensing strategy can berealized
as an observer of the nonlinear dynamical system. Two observers are subsequently
introduced. The rst observer, based on the result of [15], has its \gain" determined
by the solution of a Lyapunov-like equation; it can be activated at any time instant
during a push. The second observer, based on Newton's method, solves for the initial
motionless object pose from three intermediate contact points during a push.
Under the Coulomb friction model, the paper copes with support friction in the plane
and/or contact friction between the nger and the object. Extensive simulations have
been done to demonstrate the feasibility of the two observers. Preliminary experiments
with an Adept robot have also been conducted. Acontact sensor has been implemented
using strain gauges. 1
Figure 1: Di erent motions of contact drawn as dots on an ellipse pushing a quadrilateral
in two di erent initial p oses.
1 Intro duction
Part sensing and grasping are two fundamental op erations in automated assembly. Tra-
ditionally, they are p erformed sequentially in an assembly task. Parts in many assembly
applications are manufactured to high precisions based on their geometric mo dels. This
knowledge of part geometry can sometimes signi cantly facilitate sensing as well as grasp-
ing. It can sometimes also help integrate these two op erations, reducing the assembly time
and cost.
Consider the task of grasping something, say, a p en, on the table while keeping your eyes
closed. Your ngers fumble on the table until one of them touches the p en and inevitably
starts pushing it for a short distance. While feeling the contact move on the ngertip, you
can almost tell which part of the p en is b eing touched. Assume the pushing nger is moving
away from you. If the contact remains almost stable, then the middle of the pen is b eing
touched; if the contact moves counterclo ckwise on the ngertip, then the right end of the
p en is b eing touched; otherwise the left end is b eing touched. Immediately, a picture of the
pen con guration has b een formed in your head so you co ordinate other ngers to quickly
close in for a grip.
The ab ove example tells us that the p ose of a known shap e may be inferred from the
contact motion on a nger pushing the shap e. To b etter illustrate this idea, Figure 1 shows
two motions of a quadrilateral in di erent initial p oses pushed by an ellipse under the same
motion. Although the initial contacts on the ellipse were the same, the nal contacts are
quite far apart. Thinking in reverse leads to the main questions of this pap er:
1. Can we determine the p ose of an ob ject with known geometry and mechanical prop-
erties from the contact motion on a single pushing nger, or simply, from a few inter-
mediate contact p ositions during the pushing? 2 Nonlinear System
State
Object
Contact Dynamics of Kinematics Pushing
Sensor Finger Implementation Input
Output
State Observer
Estimate
Figure 2: Anatomy of pushing.
2. Can we determine any intermediate p ose of the ob ject during the pushing?
3. Furthermore, can we estimate the motion of the ob ject during the pushing?
In this pap er, we will give armative answers to the ab ove questions in the general
case. To accomplish this, we will characterize pushing as a system of nonlinear di erential
equations based on its dynamics. As shown in Figure 2, the state of the system will include
the con gurations p ositions, orientations, and velo cities of the nger and ob ject during
the push at any time instant. The system input will b e the acceleration of the nger. The
system output will b e the contact lo cation on the nger sub ject to the kinematics of contact.
This output will be fed to nonlinear observers, which serve as the sensing algorithms, to
estimate the ob ject's p ose and motion.
Section 2 cop es with the dynamics of pushing and the kinematics of contact, deriving a
system of di erential equations that govern the ob ject and contact motions while resolving
related issues such as supp ort friction in the plane and the initial ob ject motion; Section 3
applies nonlinear control theory to verify the soundness of our sensing approach to be pro-
p osed, establishing the lo cal observabilityof this dynamical pushing system from the nger
contact; Section 4 describ es two nonlinear observers which estimate the ob ject p ose and
motion at any instant and at the start of pushing, resp ectively, and which require di erent
amounts of sensor data; Section 5 extends the results to incorp orate contact friction b etween
the nger and the ob ject; Section 6 presents simulations on b oth observers and the imple-
mentation of a contact sensor, demonstrating that three intermediate contact p oints often
suce to determine the initial p ose for the ngers and ob jects tested; Finally, Section 7
summarizes the pap er and outlines future work. 3
1.1 Related Work
Our work is grounded in rob otics where an abundance of previous work exists. It also draws
up on the part of nonlinear control theory that concerns nonlinear observability and observers.
1.1.1 Rob otics
Dynamics of sliding rigid b o dies was treated by MacMillan [36] for non-uniform pressure
distributions, and by Goyal et al. [19] using geometric metho ds based on the limit surface
description of friction. Howe and Cutkosky [26] exp erimentally showed that the limit sur-
face only approximates the force-motion relationship for sliding b o dies and discussed other
simpli ed practical mo dels for sliding manipulation.
Mason [37] pioneered the study of the mechanics of pushing using quasi-static analysis,
predicting the direction in which an ob ject b eing pushed rotates and plotting out its in-
1
stantaneous rotation center. For unknown centers of friction, Alexander and Maddo cks [2]
reduced the problem of determining the motion of a slider under some applied force to the
case of a bip o d, obtaining analytical solutions for simple sliders. The problem of predicting
the accelerations of multiple 3D ob jects in contact with Coulomb friction has a nonlinear
complementarity formulation due to Pang and Trinkle [40]; the existence of solutions to
mo dels with sliding and rolling contacts has b een established.
Montana [38] derived a set of di erential equations describing the motion of a contact
p ointbetween two rigid b o dies in resp onse to a relative motion of these b o dies, and employed
these equations to sense the lo cal curvature of an unknown ob ject and to follow its surface
while steering the contact p oint to some desired lo cation on the end e ector. The kinematics
of spatial motion with p oint contact was also studied by Cai and Roth [6] who assumed a
tactile sensor capable of measuring the relative motion at the contact p oint. The sp ecial
kinematics of two rigid b o dies rolling on each other was considered by Li and Canny [33]in
view of path planning in the contact con guration space. In our work, contact kinematics
is derived directly from the absolute velo cities of the nger and the ob ject rather than from
their relative velo city at the contact. Also we are concerned with a nger and ob ject only
in the plane not in 3D.
Part of our motivation came from the blind grasping task at the b eginning of the pap er.
The caging work by Rimon and Blake [43] is concerned with constructing the space of all
con gurations of a two- ngered hand controlled by one parameter that con ne a given 2D
ob ject; these con gurations can lead to immobilizing grasps by following continuous paths
in the same space. This work requires an initial image of the ob ject taken by a camera.
Work related to caging includes parts feeder design [42] and xture design [5]. In this pap er,
we are concerned with how to \feel" a known ob ject using only one nger and how to infer
its p ose and motion information rather than how to constrain and grasp the ob ject using
multiple ngers.
A larger part of the motivation of our work was from parts orienting. Orienting mechan-
ical parts was studied early on by Grossman and Blasgen [22]. They used a vibrating box
to constrain a part to a small nite number of p ossible stable p oses and then determined
1
The center of friction is the centroid of the pressure distribution. 4
the particular p ose by a sequence of prob es using a tactile sensor. Inspired by their result,
Erdmann and Mason [12] constructed a planner that employs sensorless tilting op erations
to orient planar ob jects randomly dropp ed into a tray, based on a simple mo del of the
quasi-static mechanics of sliding. Utilizing the theory of limit surfaces [19], Bohringer et
al. [4] develop ed a geometric mo del for the mechanics of an array of micro electromechanical
structures and showed how this structure can be used to uniquely and eciently align a
part up to symmetry. Goldb erg [17] showed that every p olygonal part with unknown initial
orientation can be oriented by a parallel-jaw gripp er up to symmetry in the part's convex
hull. He constructed an algorithm with sub-cubic running time as a pro of of sensorless parts
orienting.
Based on the limit surface mo del and quasi-static analysis, Lynch et al. [35] conducted
active sensing of an ob ject's center of mass during pushing from tactile feedback; and de-
velop ed a control system that translates and orients ob jects. Their work assumes known
contact edge and do es not quantitively estimate the ob ject's motion due to the quasi-static
nature. Also applying quasi-static analysis, Akella and Mason [1] describ ed a complete op en-
lo op planner that can orient and translate p olygonal ob jects in the plane by pushing with a
straight fence.
In our previous work [29], weintro duced the metho ds of cone inscription and p oint sam-
pling that compute the p oses of known shap es from a continuum and a nite number of
p ossibilities, resp ectively, using simple geometric constraints such as coincidence and con-
tainment. Paulos and Canny [41] studied the problem of nding optimal p oint prob es for
re ning the p ose of a p olygonal part with known geometry from an approximate p ose; they
revealed that this problem is dual to the grasping problem of computing optimal nger
placements and gave an ecient near-optimal solution.
Mo del-based recognition and lo calization traditionally solve a constraint satisfaction
problem by searching for a consistent matching b etween sensory data and mo del s . This is
often conducted by pruning the search tree sub ject to pure geometric constraints. Grimson
and Lozano-P erez [21] used tactile measurements of p ositions and surface normals on a 3D
ob ject to identify and lo cate it from a set of known 3D ob jects, based on the geometric
constraints imp osed by these tactile data. Gaston and Lozano-P erez [14] showed how to
identify and lo cate a p olyhedron on a known plane using tactile information that includes
contact p oints and the ranges of surface normals at these p oints. Grimson [20] advo cated
that mo del-based recognition and lo calization should be regarded as a constraint satisfac-
tion problem that searches for a consistent matching between sensory data e.g., 2D and
mo del s e.g., 3D under some geometric constraints.
A ma jor di erence between the sensing approach describ ed in this pap er and the listed
previous work is that our approach combines geometric constraints with dynamics of ma-
nipulation. On the one hand, this would relieve the requirement for sucient geometric
constraints and hence lessen the hardware load . On the other hand and more imp ortantly,
the involvement of dynamics extends sensing to dynamic p ose and motion estimation. Sub-
sequently, the to ols whichwe will apply are from nonlinear control theory rather than from
AI and computational geometry.
One feasible implementation for contact detection is to employ a force or tactile sen-
sor. The pap er by Salisbury [44] prop osed the concept of ngertip force sensing with an 5
approach for determining contact lo cations and orientations from force and moment mea-
surements. Fearing and Binford [13] designed a cylindrical tactile sensor to determine the
principal curvatures of an ob ject through rolling contact. Based on continuum mechanics
and photo elastic stress analysis, Cameron et al. [7] built a tactile sensor using a layer of
photo elastic material along with its mathematical mo del. Allen and Rob erts [3] deployed
rob ot ngers to obtain a number of contact p oints around an ob ject and then t in a least
squares manner the data to a sup erquadric surface representation to reconstruct the ob-
ject's shap e. Howe and Cutkosky [25] intro duced dynamic tactile sensing in which sensors
capture ne surface features during motion, presenting mechanical analysis and exp erimental
p erformance measurements for one typ e of dynamic tactile sensor|the stress rate sensor.
1.1.2 Nonlinear Control
The theoretical foundation of our work comes from the part of control theory concerned with
the observability and observers of nonlinear systems. For a general intro duction to nonlinear
control theory, we refer the reader to Isidori [27] and Nijmeijer and van der Schaft [39].
Hermann and Krener [24] rst studied observability using the notion of observation space.
We will use their observability rank condition to show that the p ose and motion of a p olygonal
ob ject pushed by a disk is lo cally observable from the disk contact. A result due to Crouch[9]
shows that an analytic system is observable if and only if the observation space distinguishes
p oints in the state space.
Luenb erger-like asymptotic observers, rst constructed by Luenb erger [34] for linear sys-
tems, remain likely the most commonly used observer forms for nonlinear systems to day.
Gauthier, Hammouri, and Othman [15] describ ed an observer for ane-control nonlinear
systems whose \gain" is determined via the solution of an appropriate Lyapunov-like equa-
tion. Their observer has a very simple form: it is a copy of the original system, together
with a linear corrective term that dep ends only on the state space dimension. Our rst p ose
observer will b e constructed using the GHO pro cedure.
Ciccarella et al. [8] prop osed a similar observer whose gain vector is controlled by the prop-
erly chosen eigenvalues of a certain matrix obtained from the original system's Brunowsky
canonical form, thus providing more freedom on optimizing the observer b ehavior. Extending
the results of GHO [15], Gauthier and Kupka [16] characterized non-ane control systems
that are observable under any input and constructed a generic exp onential observer for these
systems.
Zimmer [46] presented a state estimator that conducts on-line minimization over some ob-
jective function. His observer, with provable convergence, iteratively uses Newton's metho d
to mo dify its state estimate every xed p erio d of time. Our second p ose observer will also
make use of Newton's metho d but we will estimate the initial motionless p ose of the ob ject,
relieving the task of evaluating the complex second order partial derivatives of the drift eld
as Zimmer had encountered. 6 yF Finger F α x F ' α ( u ) ( s ) Object B vF β β' F y ω B xB
O v dp p
vp
Figure 3: Finger F translating and pushing ob ject B .
1.2 Notation
We abide by the following conventions on notation in this pap er. Every vector x is a column
T
vector written as x ;:::;x for some variables x ;:::;x . The derivative of a vector
1 n 1 n
dx dx
T T
n
1
;:::; . function x t = x t ;:::;x t with resp ect to t is denoted by dx=dt =
1 n
dt dt
T
The gradient of a scalar function y x , where x = x ;:::;x , is a row vector @y=@x =
1 n
@y @y
T
;:::; . The partial derivative of a vector eld f x = f x ;:::;f x with
1 m
@x @x
n
1
@f
T
i
resp ect to vector x = x ;:::;x is a m n matrix, given by @ f =@ x = .
1 n
ij
@x
j
Toavoid anyambiguity, the notation `_ ' means di erentiation with resp ect to time, while
0
the notation ` ' means di erentiation with resp ect to some curve parameter. For example,
du d
0
_
gives the velo city of a p ointmoving on a curve u . The cross pro duct of = u_ =
du dt
twovectors e.g., v is treated as a scalar wherever ambiguitywould not arise. A scalar
in a cross pro duct e.g., the angular velo city ! in ! acts as a vector of equal magnitude
and orthogonal to the plane.
2 Motion of Contact
Throughout the pap er we consider the two-dimensional problem of a translating nger F
pushing an ob ject B . Coulomb's friction law is assumed and the co ecient of supp ort
friction, that is, friction between B and the plane, is everywhere . For simplicity, let us
assume uniform mass and pressure distributions of B . Let us also assume frictionless contact
between F and B at present and deal with contact friction exclusively in Section 5. Let v
F
be the velo city of F , known to F 's controller, v and ! the velo city and angular velo city of
B , resp ectively, all in the world co ordinate frame Figure 3 .
Let F 's b oundary be a smo oth curve and B 's b oundary be a piecewise smo oth closed
curve such that u and s are the two p oints in contact in the lo cal frames of F and
B , resp ectively. Following convention, moving counterclo ckwise along and increases u
and s, resp ectively. Assume that one curve segmentof stays in contact with throughout 7
the pushing.
2.1 Dynamics of Pushing
That F and B maintain contact imp oses a velo city constraint:
0
0
v + u_ = v + ! R + R s;_ 1
F
!
cos sin
where R = is the rotation matrix asso ciated with the orientation of
sin cos
B , which is determined by u, s, and the orientation of F . Newton's and Euler's equations
on rigid b o dy dynamics are stated as:
Z
^ _
F + g v dp = mv ; 2
p
B
Z
^
R F + R p g v dp = I !;_ 3
p
B
where F is the contact force acting on B , g the acceleration of gravity, m the mass, the mass
density, and I the angular inertia ab out the center of mass O all of B . Here v = v + ! R p
p
v
p
2
^
is the velo cityofp 2 B and v = its direction.
p
kv k
p
With no friction at the contact p oint, F acts along the inward normal of B :
0
F R = 0; 4
0
R F > 0: 5
Finally, the normals of F and B at the contact are opp osite to each other; equivalently, we
have
0
0
R = 0;
6
0
0
R < 0:
Given the nger motion v , there are seven equations 1 , 2 , 3 , 4 , and 6 with
F
3
seven variables u; s; ! ; v , and F . From these equations, we are now ready to derive the
di erential equations for u; s; ! , and v .
q
R
I m
and = the area and radius of Let a be the acceleration of F , A = dp =
F
B
m
R
0 0
^ ^
R R p v + v dp an integral asso ciated gyration of B , resp ectively, and =
p p
B
with supp ort friction. We have
2
That F is translating implies either v 6=0 or ! 6= 0 after the pushing starts. So v can vanish over at
p
most one p oint p 2 B , which will vanish in the integrals in equations 2 and 3 .
3
Note that equations 1 and 2 and variables v and F are each counted twice. 8
Theorem 1 Consider the pushing system described by 1 { 6 . The points of contact evolve
according to
0 00
0 2 0 0
R ! + R v + ! R v
F
u_ = ; 7
0 0 00
0 00 0 2 0
R R +k k R
0 0
0 2 0 00 0
k k R ! + R v + ! R v
F
s_ = ; 8
0 0 00
0 00 0 2 0
R R +k k R
and the object's angular acceleration and acceleration are
g
0
0 00 0 00 0
u_ v v + a u_ R + ! R + R s_ ! +
F F
0
A
!_ = ;
2
0
0
R +
0
9
0
2
A !_ R g
: 10 v_ =
0
A
0
Pro of Taking the dot pro ducts of with b oth sides of 1 and rearranging terms there-
after, we obtain
0
0 2 0 0
k k u_ R _s = v + ! R v :
F
Next di erentiate b oth sides of 6 :
0 00 0
00 0 0
R _u + R _s + R ! = 0:
Immediately,we solve for u_ and s_ from the two equations ab ove and obtain 7 and 8 .
Now we move on to derive the di erential equations for v and ! . First take the cross
0 0
pro ducts of R with b oth sides of 3 , eliminating the resulting term that contains F R
and substituting 2 in after term expansion:
0 0
_
mv g = R I !:_
Here the term
Z
0 0
^ ^
= R R p v + v dp; 11
p p
B
_
when multiplied by g , combines the dynamic e ects of friction. Thus we can write v in
the form of 10 .
0
0 0
Taking the cross pro ducts of with b oth sides of 1 and cancelling the term R
according to 6 , we have after a few more steps of term manipulation
0 0
v v = R !: 12
F 9
Di erentiating b oth sides of 12 yields
0
00 0 00 0 0
_
u_ v v + a v = u_ R + ! R + R s_ ! + R _!: 13
F F
Finally, substituting 10 in 13 gives us 9 . 2
Substitute 7 and 8 into 9 and the resulted di erential equation into 10 . We have
thus obtained the di erential equations of ! and v which, along with 7 and 8 , form a
system of ordinary di erential equations ODEs . This system is numerically solvable for
u; s; ! , and v . Without any ambiguity,we also let 9 and 10 refer to their corresp onding
di erential equations.
Our derivation of the di erential equations 7 { 10 is correct only if the denominators
on their right hand sides do not vanish. It is easy to show that these denominators vanish
0
_
only if = 0, or equivalently, R F = 0. Furthermore, the limits of !_ and v given
0
by equations 9 and 10 , resp ectively, as ! 0, are equal to their degenerate forms
0
derived under the condition =0, resp ectively. See [28].
The motion of B is independent of its mass density , as seen from 7 , 8 , 9 , and 10
or directly from 2 and 3 .
If and are unit-sp eed curves with curvatures and at the contact p oint, resp ec-
4
tively, such that + 6=0, then equations 7 and 8 are simpli ed to
0
! + v + ! R v
F
; 14 u_ =
+
0
! + v + ! R v
F
s_ = : 15
+
1
5
and =0. For example, let be a circle with radius r and a p olygon. Hence =
r
!
= !r. During a push of time t, the contact moves an arc of length Wehave u_ =
Z Z
t t
udt_ = r !dt = r
0 0
on , which can be immediately veri ed from the tangency between and .
2.2 Integral of Supp ort Friction
To numerically integrate 7 { 10 , it is necessary to evaluate the integral given by 11
which represents the e ect of supp ort friction on dynamics. Two-dimensional numerical
integration of can be very slow. However, by cho osing prop er p olar co ordinates we can
reduce the evaluation of to one variable integration, and if B is p olygonal, obtain the closed
form of .
When the motion of B is pure translation ! = 0 , the evaluation is easy:
0
^
= Av : 16
4
The term + is the 2-dimensional case of the relative curvature form intro duced by Montana [38].
5
Here is the curvature of the p olygon edge in contact with . We assume that the nger will not b e
in contact with anyvertex during the pushing. 10 φ
BB _vy _vx ()- ωω, : instantaneous rotation center
r1 ( φ )
B ω
-1 BB O R v = ( v x , v y )
r2 ( φ )
B B T
Figure 4: The instantaneous rotation center v =! ; v =! of ob ject B with velo city
y x
B B T
v ;v and angular velo city ! 6= 0 ab out its center of mass O . The integral is
x y
evaluated in p olar co ordinates with resp ect to the i.r.c..
So we fo cus our discussion on the case ! 6=0. The integral can be evaluated in the p olar
co ordinates with resp ect to the instantaneous rotation center of B intro duced b elow.
Let us rst express in terms of B 's moving b o dy frame at its center of mass O :
Z Z
B B
0 0
^ ^
p v dp + R v dp; 17 = R
p p
B B
B 1
where v = R v + ! p is the velo city at p 2 B in the body frame. At the moment,
p
B B T B B T 2 B B T
B is rotating ab out the p oint x ;y = ! v ;v =! = v =! ; v =! , called the
0 0 x y y x
instantaneous rotation center i.r.c. , as shown in Figure 4. For convenience and clarity,
we only illustrate the case where B is convex. The evaluation should be straightforwardly
generalized to the case where B is concave.
T T
Anyray at angle from the i.r.c. has at most twointersections ; r and ; r ,
1 2
r < r , with the ob ject b oundary. Every p oint p on the ray is instantaneously
1 2
B
T
^
moving along the same direction v = sin ; cos if ! > 0, or along the direction
p
B
T
^
= sin ; cos if ! < 0. The two subintegrals in 17 now reduce to one-variable v
p
6
integrals in the p olar co ordinates:
Z
B
^
I = v dp
1
p
B
!
Z Z
r
2 2
sin
= rdr d
cos
r
1 1
!
Z
2 2
2
r r
sin
2 1
= d ; 18
cos
2
1
Z
B
^
p v dp I =
2
p
B
6
If the i.r.c. is in the interior of B , let =0, =2 , and r =0.
1 2 1 11
! !! !
Z Z
B
r
2 2
x cos sin
0
= + r rdrd
B
y sin cos
r
1 1
0
Z
2 2 3 3
2
r r r r
2 1 2 1
B B
= x cos + y sin + d :
0 0
2 3
1
For p olygonal shap es, the closed forms of the ab ove two integrals are given in [28]; for
most other shap es, these integrals can only b e evaluated numerically.
2.3 Initial Motion
In order to numerically integrate equations 7 { 10 , it is necessary to determine the initial
_
accelerations v and !_ of B from the nger acceleration a and the con gurations of F and
F
7
B .
At the start of pushing, b oth the nger F and the ob ject B are motionless; that is, we
have
v 0 = v =0; ! 0 = ! =0; and v 0 =0:
0 0 F
Plugging the ab ove into 7 and 8 yields the initial contact velo cities:
u_ 0 = 0 and s_ 0 =0:
We only consider the non-degenerate case where the contact normal N do es not pass
through the center of mass O as otherwise the initial accelerations can b e easily determined.
In other words, we deal with !_ 6= 0. The frictional force f at p oint p 2 B is opp osed to
0
p
the direction of relative motion [18], which, at the start of pushing, is the direction of the
acceleration
_ _
v 0 = v + !_ p + ! ! p
p 0 0 0 0
_
= v + !_ p
0 0
_
v
0
+1 p : = !_
0
!_
0
By a simple argument, the sign of !_ must agree with its sign were there no friction; hence
0
^
_
it is easily determined. Consequently, v 0 , f ,
p
p
Z Z
0 0
^ ^
_ _
= R R p v 0 dp + v 0 dp
0 p p
B B
v_
0
b ecome functions of . Thus 10 can b e rewritten as
!_
0
0
v_
2
0
A !_ R g
0 0
!_
0
_
19 v =
0
0
A
7
In this section, our fo cus is on simulating the start of pushing. Hence we temp orarily assume the initial
con guration of B is known. Later in Section 4.2, we will see how to use such simulation to solve for the
initial p ose of B using Newton's metho d. 12
8
at t =0. Meanwhile, it follows from 13 that
0
a
F
!_ = : 20
0
_
v
0
0 0
R +
!_
0
Dividing b oth sides of 19 by !_ and substituting 20 in, we get the following equation in
0
v_
0
:
!_
0
_
v
0
0 0
R +
!_
0
v_
2 0
0
A R g
0
!_
0
0
_
v
0
a
F
= : 21
0
!_ A
0
v_ v_
0 0
by Newton's metho d using the value of for = 0, Equation 21 is solvable for
!_ !_
0 0
obtainable directly from 9 and 10 , as an initial estimate. Intuitively, the metho d iterates
until the computed acceleration and angular acceleration agree with what would b e yielded
under Newton's lawby the nger acceleration and the frictional force, the latter of whichin
turn dep ends on the accelerations themselves under Coulomb's lawof friction.
v_
0
required for the iterations can be evaluated numerically The partial derivative @ =@
0
!_
0
_
or using its closed form when B is p olygonal [28]. Hence !_ and v are determined from 19
0 0
and 20 .
2.4 Contact Breaking
The only constraint that was left out in the derivation of the di erential equations 7 { 10
is inequality 5 . This constraint, however, is used for checking when the contact between
0
the nger and the ob ject breaks. More sp eci cally, the contact breaks when R F < 0.
3 Lo cal Observability
In the previous section we saw that the kinematics of contact and the dynamics of pushing
are together determined by a system of nonlinear ordinary di erential equations 7 { 10 .
A state of this nonlinear system consists of u and s, which determine the contact lo cations
on the nger and on the ob ject, resp ectively, the ob ject's angular velo city ! ,velo city v , and
orientation ; the input is the nger's acceleration a , generated by the controller of the
F
nger; and the output is u, rep orted by a tactile sensor mounted on the nger. The sensing
task b ecomes to \observe" s from u, which, as suggested by the system equations, is no
easier than to \observe" the whole state of the system.
In this section we shall study lo cal observability of one instantiation of the ab ove system in
which the nger is circular and the ob ject is p olygonal. This typ e of pushing is representative
in real manipulation tasks. First of all, weintro duce the notion of nonlinear observabilityas
8
We here make a simpli cation by identifying the co ecient of static friction with the co ecient of kinetic
friction . 13
well as a theorem ab out lo cal observability; next, we show that the instantiation is lo cally
observable. It will then not b e dicult to see that these results can generalize to many other
nger and ob ject shap es.
3.1 Observability of a Nonlinear System
Let us consider a smo oth ane or input-linear control system together with an output
map:
m
X
m
_
x = f x + u g x ; u ;:::;u 2 U < ; 22
i 1 m
i
i=1
y = h x ;
T n
where x = x ;:::;x is the state in a smo oth n-dimensional manifold M < called the
1 n
T
state space manifold , f ; g ;:::;g are smo oth vector elds on M , and h = h ;:::;h :
1 k
1 m
k
M ! < is the smo oth output map of the system. We call f the drift vector eld and
g ;:::;g the input vector elds. In the system, u ;:::;u are the inputs, called the controls,
1 m
1 m
over time whose Cartesian pro duct range U de nes the system's input space. At state x,
f x is a tangent vector to M representing the rate of change of x when there is no input,
while g x for 1 j m is a tangent vector showing the change of such rate under unit
j
input of u .
j
Throughout we are only concerned with the class of controls U that consists of piecewise
9
constant functions that are continuous from the right. We call these controls admissible.
The system with constant controls, or no input elds, equivalently, is said to b e autonomous.
Denote by y t; x ; u , t 0, the output function of the system with initial state x and
0 0
under control u. Two states x ; x 2 M are said to b e indistinguishable denoted by x I x
1 2 1 2
if for every admissible control u the output functions y t; x ; u and y t; x ; u , t 0 are
1 2
identical on their common domain of de nition. The system is observable if x I x implies
1 2
x = x .
1 2
To derive a condition on nonlinear observability, the ab ove de nition of \observable" is
lo calized in the following way. Let V M b e an op en set containing states x and x . These
1 2
V
two states are said to be V -indistinguishable, denoted by x I x , if for any T > 0 and any
1 2
constant control u : [0;T] ! U such that x t; x ; u ; x t; x ; u 2 V for all 0 t T , it
1 2
follows that y t; x ; u = y t; x ; u for all 0 t T on their common domain of de nition.
1 2
The system is local ly observable at x if there exists a neighborhood W of x such that for
0 0
V
every neighborhood V W of x the relation x I x implies that x = x . The system is
0 0 1 0 1
called local ly observable if it is lo cally observable at every x 2 M . Figure 5 illustrates lo cal
0
observability for the case of one output function.
A one-form on M is a real-valued and p ointwise linear function on the set of all tangent
vectors to M . The cotangent space of M at state x includes all the one-forms on M instan-
@h @h
j j
tiated by x. In particular, it includes for 1 j k the gradientof h : dh = . ;:::;
j j
@x @x
n
1
The Lie derivative of function h : M !< along a vector eld X on M , denoted by L h ,
j X j
is the directional derivative dh X =dh X . For simplicity, let notation L L L h
j j X X X j
1 2
l
9
So that U is closed under concatenation. 14 y
h ( x ( t,x 1 ,u ) ) x x( t,x 1 ,u ) 1 t1 W V t0 M x0 h( x ) 0 h ( x ( t,x 0 ,u ) ) x( t,x 0 ,u )
t0 t1 t
Figure 5: Lo cal observability at state x . Given the state space M , W M is some
0
neighborhood of x . For any neighborhood V W of x , x is V -distinguishable from all
0 0 0
other states in V . More sp eci cally, for any state x 6= x in V , there exists a constant
1 0
admissible control u such that the two state tra jectories x t; x ; u and x t; x ; u will yield
0 1
di erent outputs b efore one of them exits V at time t .
0
stand for the rep eated Lie derivative L L ::: L h ::: with resp ect to vector elds
X X X j
1 2
l
X ;:::;X ;X . The observation space O of system 22 is the linear space over < of
l 2 1
functions on M that includes h ;:::;h , and all rep eated Lie derivatives
1 k
L L L h ; j =1;:::;k; l =1; 2;:::
X X X j
1 2
l
where X 2 ff ; g ;:::;g g, 1 i l . It is not dicult to show that O is also the linear
i
1 m
space of functions on M that includes h ;:::;h , and all rep eated Lie derivatives
1 k
L L L h ; j =1;:::;k; l =1; 2;:::
Z Z Z j
1 2
l
where
m
X
Z x =f x + u g x ; 23
i ij
j
j =1
for some p oint u = u ;:::;u 2 U .
i i1 im
The observation space shall b e b etter understo o d with the notion of integral curve. Given
a nonlinear system
_
x = Z x ;
de ned by some vector eld Z on the state space M , the integral curve t is the solution of
x
0
the system satisfying the initial condition 0 = x . For every b ounded subset M M ,
x 0 1
0
there exists an interval t ;t 3 0 on which the integral curve t is well-de ned for all
1 2 x
0
t 2 t ;t . This allows us to intro duce on M a set of maps, called the ow,
1 2 1
t
Z : M ! M; t 2 t ;t ;
1 1 2
x 7! t :
0 x
0 15
Now cho ose inputs of system 22 such that it is driven by a sequence of vector elds
Z ;:::;Z of form 23 for small time t ;:::;t , resp ectively. The outputs of the system
1 p 1 p
at t + + t time are
1 p
t
p 1
t t
p
1
h Z Z Z x ; for i =1; :::; k :
i 0
p 1
p 1
Di erentiate these outputs sequentially with resp ect to t ;t ;:::;t at t =0;t =0;:::,
p p 1 1 p p 1
t = 0 yields L L :::L h x , for i = 1;:::;k. Hence we see that the observation
1 Z Z Z i 0
p
1 2
space in fact consists of the output functions and their derivatives along all p ossible system
tra jectories.
The observability codistribution at state x 2 M , denoted dO x , is de ned as:
dO x = spanf dH x j H 2Og:
We are ready for a main theorem concerning lo cal observability:
Theorem 2 Herman and Krener System 22 is local ly observable at state x 2 M if
0
dim dO x =n.
0
The equation dim dO x = n is called the observability rank condition. Pro ofs of the
0
ab ove theorem can be found in [24] and [39, pp. 95{96]. Basically, to distinguish between
a state and any other state in its neighborhood, it is necessary to consider not only the
output functions but also their derivatives along all p ossible system tra jectories. The rank
condition ensures the existence of n output functions and/or derivatives that together de ne
a di eomorphism on some neighborhood of the state, which in turn ensures that the state
is lo cally distinguishable.
3.2 The Disk-Polygon System
u u
T
Now we study the case in which nger F is a disk b ounded by = r cos ; sin and
r r
ob ject B is a simple p olygon. The interior of one edge e of B maintains contact with F
10
throughout the pushing. We assume that e is known since lo cal observability is concerned,
and since a sensing strategy can hyp othesize all edges of B as the contact edge and verify
them one by one. Let h b e the distance from the centroid O of B to e. Cho ose s as the signed
distance from the contact to the intersection of e and its p erp endicular through O such that
s increases monotonically while moving counterclo ckwise with resp ect to B 's interior on e.
11
See Figure 6. The orientation of B is = u=r =2. The tangent and normal of F at
u u u u
0 T 00 T
the contact are T = = sin ; cos and N = r = cos ; sin , resp ectively.
r r r r
The system is governed by the following nonlinear equations as sp ecial cases of 7 { 10 ,
12
resp ectively:
u_ = !r;
10
This is easily realizable in a real pushing scenario.
11
. for some constant Given a di erent contact edge e it follows = u=r =2+
e 1 e
1 1
12
These equations assume that O and the disk center are on di erent sides of e. Otherwise the term r + h
_
in the equations for s;_ !;_ v need to b e replaced by r h. 16 O
T h s N vF __u r e
r
Figure 6: A circular nger pushing a p olygonal ob ject.
s_ = T v v ! r + h ;
F
s g
2
!_ = ! r + h 2!T v v T a T ; 24
F F
2 2 2 2
s + A s +
2
g
2
_
v = N a ! r + h +2!T v v N T T
F F
2 2
s + As
s g
N N;
2 2
s + A
where
= sR I + I N 25
1 2
is the integral of friction reduced from 17 with I and I given by 18 . We will refer to 24
1 2
and its future variations as the disk-polygon system.
Of all the variables and constants in system 24 , only the height h of the contact edge
from the p olygon's center of geometry and the contact lo cation s on the edge enco de
the geometry of the contact. Supp ose the p olygon assumes the degeneracy that two of its
edges have the same height. Then every pair of p oints on these two edges, resp ectively,
and with the same s value, would result in exactly the same system b ehavior. The system
cannot distinguish between such pair of contact p oints, or subsequently, the corresp onding
two di erent p oses, just from the disk contact u.
The relative orientation of the p olygon to the disk, determined by u, app ears in the
_
equations for s;_ !_ , and v , thereby in b oth system kinematics and dynamics. The relative
p osition, determined by s, however, app ears only in the system dynamics. That s do es not
directly a ect the kinematics is due to that lo cal geometry on the contact edge is everywhere
the same, with zero curvature. However, this is not true for curved ob jects.
To apply Theorem 2 to show that system 24 is lo cally observable, we rst need to
rewrite it into the form 22 of an ane system. For convenience, we express v in terms of
T
the Frenet frame at the disk contact de ned by the tangent T and normal N : v = v ;v ,
T N
where v = v T and v = v N . Also express the disk velo city v and acceleration a in
T N F F 17
T T
the same frame as v ;v and a ;a , resp ectively. We nd that v dep ends on s,
F F F F N
T N T N
! , and v by taking the dot pro duct of N with the velo city constraint 1 :
F
N
v = v + s! :
N F
N
dT
From the ab ove equation, = !N , and 24 we have
dt
d v T
v_ =
T
dt
dT
_
= v + v T
dt
g
= !v
N T
As
g
2
= !v + s! ;
F T
N
As
where = T .
T
System 24 is now rewritten as
_
x = f x +a g x +a g x : 26
F F
T N
T N
T
The state x of the system b ecomes u; s; ! ; v ;v ;v with six variables in total; the
T F F
T N
inputs are the acceleration comp onents a and a along the contact tangent and normal,
F F
T N
T
resp ectively; and the output is a triple y = u; v ;v . The drift and input elds are
F F
T N
given by
0 1
!r
B C
v v ! r + h
B C
T F
T
B C
s g
B C
2
! r + h 2! v v
B C
T F N
T
2 2 2 2
B C
s + A s +
B f x = C ;
g
B C
2
!v + s!
B C
F T
N
B C
As
B C
0
@ A
0
27
T
g x = 0; 0; 0; 0; 1; 0 ;
T
!
T
s
: ; 0; 0; 1 g x = 0; 0;
N
2 2
s +
Theorem 3 The disk-polygon system 26 is local ly observable.
Pro of By Theorem 2 it suces to show that the observability co distribution dO has
rank 6 at every state. Here the observation space O consists of the outputs u; v ;v and
F F
T N
their rep eated Lie derivatives. Wecho ose from O the following functions and write out their 18
di erentials:
du = 1; 0; 0; 0; 0; 0 ;
dv = 0; 0; 0; 0; 1; 0 ;
F
T
dv = 0; 0; 0; 0; 0; 1 ;
F
N
L u = du f = !r; dL u = 0; 0;r; 0; 0; 0 ;
f f
!
2 2
s s
L L u = r ; dL L u = 0;r ; 0; 0; 0; 0 ;
g f g f
N N
2 2 2 2 2
s + s +
!
2 2
r r + h s s
= 0; L L L L u = r r + h ; dL L L L u ; 0; 0; 0; 0 :
g f g f g f g f
N N N N
2 2 3 4
s=
s + 4
Thus du; dv ;dv ;dL u, and dL L u or dL L L L u when s = span the cotangent
F F f g f g f g f
T N N N N
T
space of the space of all p ossible 5-tuples u; s; ! ; v ;v . It suces to nd one more
F F
T N
function in O whose partial derivative with resp ect to v will not vanish.
T
Such a task is quite easy, for we have
2 2
@ L L L u s
f g f
N
; = r
2 2 2
@v s +
T
r r + h @
L L L L L u = :
f g f g f
N N
4
@v 4
s=
T
In summary, the observability co distribution dO is spanned by du; dv ;dv ;dL u,
F F f
T N
dL L u and dL L L u or dL L L L u and dL L L L L u when s = and thus
g f f g f g f g f f g f g f
N N N N T N
attains full rank. 2
The ab ove pro of in fact constructs several control sequences which, when applied for
in nitesimal amounts of time, will distinguish b etween di erent states in any neighborhood.
Assuming s 6= , one of the functions u, v , v , L u, L L u, and L L L u must have
F F f g f f g f
T N N N
di erent values in any two di erent states close enough as guaranteed by the observability
rank condition. Note that L u is in fact the di erential output under zero control. Since
f
1 1
L L u may be written as L L L u L u, one of these two functions must dis-
g f f +g f g f f
N N N
2 2
tinguish the two states if L L u do es. Obviously, L L u or L L u is realizable in
g f f +g f f g f
N N N
an arbitrarily small amount of time by the control sequence starting with zero control and
ending with a = 1 or 1 . The case with function L L L u is similar.
F f g f
N N
Moreover, the pro of reveals the relative \hardness" of observing the state variables, es-
p ecially u; s, and ! . The disk contact u constitutes the system output and thus is the easiest
to observe. The angular velo city ! of the p olygon needs to b e obtained from the rst order
derivative of u. The p olygon contact s, the hardest of the three to observe, requires a Lie
derivativeofthe second order or ab ove, which is obtained using two or more controls.
Supp ort friction do es not a ect the lo cal observability of the disk-p olygon system, as
none of the di erentials chosen in the pro of to span dO involve the integral or any of its
partial derivatives. 19
The pro of makes use of the input vector eld g but not g , which suggests that pushing
N T
along a tangential direction is unnecessary for the purp ose of lo cal observability. Intuition
tells us that pushing along the contact normal will more likely helps the disk observe the
p olygon.
We conjecture that the autonomous version of the system under a = 0 is lo cally
F
observable at all except a nite number of states. Although it seems much more dicult to
2 3
prove the linear indep endence of du, dv , dv , dL u, dL u, and dL u at every state, this
F F f
f f
T N
conjecture will b e supp orted by our simulation results later in Section 6.
4 Pose Observers
With lo cal observability, we can view sensing strategies as nonlinear observers for the disk-
p olygon system 26 or for the general pushing system 7 { 10 . An observer of a nonlinear
system is a system whose state converges to the state of the original system. The input of
the observer consists of the input as well as the output of the original system.
Luenb erger-like asymptotic observers [34] for nonlinear systems are often designed through
linearization. The disk-p olygon system 26 , however, cannot be linearized for we have
2 2
s s
L L L L u = r r + h ;
g f g f
N N
2 2 3
s +
violating one of Nijmeijer's necessary conditions [39, p. 156] on linearization. Another ap-
proach of observer design transforms the original system into a linear system mo dulo an
output injection [32]. The necessary conditions for a nonlinear system to admit linear ob-
server error dynamics are rather restrictive and hardly satis ed by the disk-p olygon system,
let alone system 7 { 10 . Even if these conditions hold, it is still quite burdensome and
sometimes imp ossible to nd explicit solutions to partial di erential equations involving
rep eated Lie brackets on which the desired co ordinate transformation must be based.
Our observer, for the disk-p olygon system only, uses a result by Gauthier, Hammouri
and Othman to b e intro duced next.
4.1 A Gauthier-Hammouri-Othman Observer
We shall apply the Gauthier-Hammouri-Othman GHO pro cedure [15] to nd an observer
for the disk-p olygon system 26 .
Theorem 4 Gauthier, Hammouri, and Othman Consider the single output nonlin-
ear and analytic system
_
x = f x ; 28
y = h x ;
de nedon n-dimensional state space manifold M . Suppose the fol lowing two conditions hold: 20
0 1
h x
B C
L h x
B C
f
B C
1. the mapping Z : x 7! z = is a di eomorphism on M ,
.
B C
.
@ A
.
n 1
L h x
f
n n 1
2. L h x can be extended from M to < by a C function that is global ly Lipschitzian
f
n
on < .
Let C = 1; 0;:::;0 . Let A = a be an n n matrix with a = 1 if i = j 1 and 0 if
ij ij
i 6= j 1, and S be the n n matrix that satis es the equation
1
T T
S A S S A + C C = 0; 29
1 1 1
where is some large enough constant. Then the system
1
@Z
1 T
_
~ ~ ~ ~
Z x S C 30 x = f x h x y
1
@ z
is an observer for 28 with error dynamics
t
3
~ ~
kx t x t k K e kx 0 x 0 k;
where K is some constant.
The pro of of the ab ove theorem given in [15] is based on standard Lyapunov arguments.
The parameter control the sp eed of the observer. The matrix S = s is the limit of
1 ij
T T
_
the stationary solution of S = S A S S A + C C as t !1, with the
t t t t
13
initial value S b eing any symmetric p ositive de nite matrix. The symmetric matrix S
0 1
can b e determined by starting from its rst row and column simultaneously and progressing
1
to higher ordinal pairs of rows and columns. We observe s = and let s = s =0, for
11 0j j 0
j =1; 2;:::;n. Then the remaining entries of S satisfy a three-term recurrence relation:
1
1
s + s ; i>1orj > 1. s =
i 1;j i;j 1 ij
The observer 30 is a copy of the original system 28 with a corrective term that do es
not dep end on system 28 but only on the dimension and the desired convergence sp eed .
The GHO observer for a general nonlinear system 22 with inputs is a copy of the original
system plus the error corrective term given in 30 . Tohave such an observer, not only must
_
conditions 1 and 2 in the ab ove theorem hold for the drift system x = f x , but also the
original system must be observable for any input.
Getting back to the disk-p olygon system 26 , we now need to consider only u; s; ! , and
v as state variables. The drift and input elds reduce from 27 to
T
0 1
!r
B C
v v ! r + h
B C
T F
T
B C
s g
2
B C
f x = ; 31
! r + h 2! v v
B C
T F N
T
2 2 2 2
B C
s + A s +
@ A
g
2
+ s! !v
T F
N
As
13
So is S for t>0 symmetric p ositive de nite.
t 21
T
g x = 0; 0; 0; 0 ;
T
!
T
s
g x = 0; 0; ; 0 :
N
2 2
s +
With u b eing the system's only output, the new co ordinates under map consist of u and
its Lie derivatives, up to the third order:
1 0 1 0
u
u
C B C B
!r
s
C B C B
C B C : ! x = B
A @ A @
rL !
!
f
2
rL !
v
T
f
2
Generally, for all except at most a nite number of states, du; rd!; rdL ! , and rdL ! are
f
f
linearly indep endent, which implies that the map is lo cally di eomorphic. The Jacobian
1
of the inverse transformation is then the inverse of the Jacobian of [45, p. 2-17]:
0 1
1
du
1
B C
@
rd!
B C
: = B C
@ A
rdL !
@ x
f
2
rdL !
f
The di erential dL ! consists of the following partial derivatives:
f
!
s g @ @L ! v
T f F
N
= 2! N ;
2 2 2 2
@u s + r A s + @u r
2 2
2g s g s @ @L !
f
2
! r + h 2! v v + = N ;
T F N
T
2 2 2 2 2 2 2 2
@s s + A s + A s + @s
g @L ! s @
f
! r + h v v = 2 N ;
T F
T
2 2 2 2
@! s + A s + @!
@L ! g @ s
f
T N; = 2!
2 2 2 2
@v s + A s + @ v
T
2
where the closed form of d on u; s; ! ; v can b e derived see [28] . The di erential dL ! ,
T
f
however, involves second order partial derivatives of whose closed forms is to o complicated
2
to obtain. Hence we cho ose to evaluate dL ! numerically.
f
Solve equation 29 under n = 4 and take the inverse of the solution:
1 0 1 0
1 1 1 1
2 3 4
4 6 4
2 3 4
C B C B
C B C B
3 2 4 1
2 3 4 5
C B C B
6 14 11 3
2 3 4 5
C B C B
1
and S = C B : C S = B
1
1
C B C B
3 10 1 6
3 4 5 6
4 11 10 3
C B C B
3 4 5 6
A @ A @
1 10 4 20
4 5 6 7
3 3
4 5 6 7
22
Finally, from 30 and 31 we obtain an GHO observer for frictionless contact:
1 0
1 0 1 0
1
_
u~
1 0 0 0
4
C B
C B C B
2
_
C B
6 0 0 r 0 s~
C B C B
C B
C ~u u : B C = f ~u; s;~ !;~ v~ a N ~u g ~s B
T F
3 N
C B
_
A @ A @
rdL ! ~u; s;~ !;~ v~
4
!~
f T
A @
2
4
_
rdL ! ~u; s;~ !;~ v~
v~
T
f
T
32
It should be noted that we did not verify condition 2 in Theorem 4. The Lie derivative
4 4
L u is generally not extendible to a globally Lipschitzian function. However, L u is lo cally
f f
Lipschitzian. So the observer should work well as long as the state estimate is close to the
real state and its tra jectory do es not exit the lo cal neighborhood in which the Lipschitz
condition holds. This will b e supp orted by the simulation results in Section 6.1.
4.2 The Initial Pose Observer
The asymptotic observer presented in Section 4.1 has two drawbacks. First, for nger and
ob ject shap es other than disks and p olygons, the computation of the Lie derivatives and
the Jacobian may b ecome a burden. Second, the observer requires a sequence of contact
lo cations on the nger to b e sensed, which may cause diculties in sensor implementation.
One sensing strategy is to observe the initial ob ject p ose. The state of the pushing
system at any time will then be determined from equations 7 { 10 given that the nger's
p ose and velo city during the pushing are known to the controller . The initial ob ject p ose
is determined by the initial contact p osition s on the ob ject b oundary. So is the contact
0
p osition u t on the nger. This fact leads to our second observer which is a variation of
the sho oting metho d for integrating ordinary di erential equations. This observer is for
system 7 { 10 in which the nger and the ob ject have general planar shap es.
For each initial ob ject contact s , there is a unique nger contact tra jectory u t
0
u t; s , t 0, as the solution to the di erential equations 7 { 10 with the initial values
0
including s . Note u 0; s = u must hold, where u is the initial nger contact. Let the
0 0 0 0
nger sense a second contact p osition u at time t > 0. Then the problem reduces to
1 1
nding a zero s of the function u t ; s u . Figure 7 depicts the contact curves resulting
1 0 1
0
from two di erent initial ob ject contacts, together with the curve segment g s =u t ; s
0 1 0
representing all p ossible nger contacts at time t resulting from any initial ob ject contact
1
in b etween.
The ro ot s of u t ; s u can b e obtained iteratively with Newton's metho d for solving
1 0 1
0
nonlinear equations. Each evaluation of this function nowinvolves solving a separate initial
value problem for the system 7 { 10 given the value of s at the present iteration step.
0
The initial p ose IP observer is also lo cal and therefore sub ject to how close the estimate
on s at the start of iteration is from the real p ose s . To globalize sensing, we provide
0
0
Newton's metho d with multiple guesses of s along the ob ject b oundary. This may yield
0
multiple solutions to u t ; s = u , as we will see in the simulation results in Section 6.2.
1 0 1
However, suchambiguities can often b e resolved by detecting a third contact u on the nger
2
at time t >t and verifying against u the nger contacts at t resulting from all ambiguous
2 1 2 1
s values.
0 23 u u( t 1 ; s 0 ) u( t 1 ; b ) u( t 1 ; b )
uu11
u( t 1 ; a ) u( t 1 ; a )
u0 t
0 t1 a s*0 b s0
a b
Figure 7: A sho oting metho d for initial p ose determination. a Two di erent nger contact
motions resulting from the initial ob ject p oses s = a and s = b, resp ectively; b p ossible
0 0
nger contacts at time t resulting from any initial p ose s 2 [a; b] constitute a curve g s =
1 0 0
u t ; s . The initial p ose observer works by intersecting g s with the line u t = u to
1 0 0 1 1
determine the real initial ob ject p ose s .
0
5 Contact Friction
This section extends the results in the previous sections to include contact friction b etween
the nger and the ob ject. Nowwe need to consider two mo des of contact: rol ling and sliding,
according as whether the contact force lies inside the contact friction cone or on one of its two
edges see Figure 8 . Each modeishyp othesized and solved; then the obtained contact force
is veri ed with the contact friction cone for consistency. This hyp othesis-and-test approach
is quite common in solving multi-rigid-b o dy contact problem with Coulomb friction. See,
for instance, Haug et al. [23].
5.1 Rolling
When rolling contact o ccurs, the contact force F may lie anywhere inside the contact friction
cone. Let b e the co ecient of contact friction. Constraint 4 for frictionless contact must
c
now be replaced by
0 0
R + R F < 0 < R R F; 33
2 2
1
where = tan is the half angle of the contact friction cone and R; ;F are de ned in
c
Section 2. Furthermore, the two p oints in contact, xed on and , resp ectively,must have
the same instantaneous velo city; that is,
v = v + ! R : 34
F
Subtracting 34 from the velo city constraint 1 on contact maintenance yields
0
0
u_ = R s:_ 35 24 Finger vF α Finger ( u ) α' α' β' β ( s ) α ( u ) β ( s ) Object 2φ vF F β' 2φ ω F Object v ω
v
a Rolling b Sliding
Figure 8: Two mo des of motion under contact friction: a rolling, in which the contact force
F p oints to the interior of the contact friction cone; b sliding, in which F p oints along one
of the edges of the cone.
We are now ready to set up the contact and ob ject motion equations for rolling.
Prop osition 5 In the problem of a translating nger pushing an object considered in Sec-
tion 2, assume contact friction between the nger and the object as wel l. In addition to the
1
notation of Section 2, let and = tan be the coecient and the angle of contact fric-
c c
tion, respectively. When the object is rol ling along the nger boundary, the pushing system
is determined by 2 , 3 , 6 , 33 , 34 , and 35 . The contact and object motions satisfy
0
0 2
R
u_ = ! ; 36
0 0 00
0 00 0 2 0
R R +k k R
0
0 2 0
k R k
s_ = ! ; 37
0 0 00
0 00 0 2 0
R R +k k R
Z
g
0
^
R p v dp R a ! s_ +
p F
A
B
; 38 !_ =
2 2
k k +
v = v ! R :
F
Pro of Equations 36 and 37 are just the sp ecial cases of equations 7 and 8 , resp ec-
tively, under the rolling constraint 34 .
Di erentiate b oth sides of 34 :
0
2
_
a = v + !_ R ! R + ! R s:_ 39
F 25
Meanwhile, substituting Newton's equation 2 into Euler's equation 3 and manipulate the
resulting terms to obtain
Z
1
_ ^
R v = I !_ + g R p v dp : 40
p
m
B
Taking the cross pro ducts of R with b oth sides of 39 and plugging 40 in, wehave after
a few steps of term expansion:
Z
1
0
2
^
I !_ + g R p v dp + k k !_ + ! s_ ; R a =
p F
m
B
from which 38 immediately follows. 2
Toinvestigate lo cal observability in the presence of contact friction, we lo ok at the same
problem of a disk pushing a p olygon considered b efore. In fact, lo cal observability for the
case of rolling can b e established more easily. Under rolling contact, v dep ends on u; s; ! :
v = v ! R
F
= v ! hN sT
F
= v + ! hT + sN : 41
F
T 14
Subsequently, a state can be denoted by x = u; s; ! . And the dynamical system 26
has simpler drift and input elds:
0 1
!r
B C
B C
!r
Z
B C
g
f = B C ;
2
B C
^
rs! + R p v dp
p
@ A
A
B
2 2 2
s + h +
!
T
h
; 42 g = 0; 0;
T
2 2 2
s + h +
!
T
s
g = 0; 0; :
N
2 2 2
s + h +
We leave to the reader the task of verifying that the di erentials du, dL u, and dL L u or
f g f
T
dL L u if s =0 are linearly indep endent.
g f
N
Theorem 6 The disk-polygon system 42 with rol ling contact between the disk and the
polygon and under support friction in the plane is local ly observable.
14
Unlike in the case of frictionless contact, here v and v are not involved in the dynamics of rolling.
F F
T N
So they are not considered as state variables. 26
The GHO observer for the rolling case has the form
0 1
_
u~
B C
_
= f ~u; s~; !~ + a T ~u g ~s + a N ~u g ~s
s~
@ A
F F
T N
_
!~
1 0 0 1
1
3 1 0 0
C B B C
2
3 0 0 r
~u u : 43
A @ @ A
3
rdL ! ~u; s;~ !~
f
The derivation of 43 is similar to that of 32 and thus omitted here.
5.2 Sliding
When sliding contact o ccurs, F must lie along one edge of the contact friction cone that makes
an obtuse angle with the sliding direction. Constraint 4 must b e accordingly replaced by
0
F R = 0; 44
where \" is determined by the sliding direction, which is hyp othesized. Rewrite con-
0 0 0
0 0
~ ~ ~
straint 44 as F R = 0 where = R . Also denote = R and
R
0 0
~ ~
~
^ ^
= R R p v + v dp. The di erential equations governing contact and
p p
B
ob ject motions are similar to those under no contact friction given in Section 2 .
Prop osition 7 In the problem of a translating nger pushing an object considered in Sec-
tion 2, assume contact friction between the nger and the object as wel l. In addition to the
1
notation of Section 2, let and = tan be the coecient and the angle of contact
c c
friction, respectively. When the object is sliding along the nger boundary, the pushing sys-
tem is determined by 1 , 2 , 3 , 5 , 6 , and 44 . The contact motions stil l fol low 7
and 8 , while the object's angular acceleration and acceleration satisfy
g
0
0 00 0 00 0
~
u_ v v + a u_ R + ! R + R s_ ! +
F F
0
~
A
!
!_ = ;
2
0
0
~
+ R
0
~
45
0
2
~
~
A !_ R g
_
: 46 v =
0
~
A
Pro of Analogous to the pro of of Theorem 1. 2
The resemblance of equations 45 and 46 to equations 9 and 10 suggests the rea-
soning on lo cal observability for the disk-p olygon system in the sliding case to resemble the 27
pro of of Theorem 3. We write the system in this case into the form of 26 and obtain its
drift and input elds:
0 1
0
B C
B C
0 1
B C
0
!r
B C
B C B C
s cos h sin
v v ! r + h
B C B C
T F
T
B C B C
2 2
B C B C
s cos hs sin + cos
B C B C
f = ; g = ;
2
N
B C B C
B C B C
sin
B C B C
2 2
B C
@ A
0
s cos hs sin + cos
B C
B C
0
0
B C
@ A
1
and g as given in 27 , where \" stands for \+" for left sliding of the p olygon and \ " for
T
" for some complicated terms. Involved calculations will reveal that right sliding, and \
du; dv ;dv ;dL u, dL L u or dL L L L u , and dL L L u or dL L L L L u
F F f g f g f g f f g f f g f g f
T N N N N N N N
r +h
again span the observability co distribution unless tan = .
Theorem 8 The disk-polygon system with sliding contact between the disk and the polygon
r +h
and under support friction in the plane is local ly observable if tan 6= .
6 Simulations and Exp eriments
We simulated the GHO observer and the IP observer by the fourth-order Runge-Kutta
integration with a stepsize corresp onding to 0.01 second 0.01s real time. The ob ject data
15 16
in our simulations included p olygons and ellipses, all of whichwere randomly generated.
Our pro ofs of the lo cal observability of the disk-p olygon system and its variations in
Sections 3.2, 5.1, and 5.2 did not rely on supp ort friction. This suggests that friction would
hardly a ect the observer's p erformance. So we set the co ecient of supp ort friction to be
uniformly 0.3. This number was also consistent with the measurements in our exp eriments,
which are to be discussed in the next section. The nger accelerations and velo cities used
in the simulations are easily achievable on an Adept rob ot. For convenience, only constant
nger accelerations were used.
The simulation co de was written in Lisp and run on a Sparcstation 20. The ma jor load
of computation turned out to have come from the evaluations of the integrals of friction and
their partial derivatives with resp ect to the ob ject p ose and velo cities . To sp eed up, these
integrals and their rst order partial derivatives were evaluated via closed forms when the
ob ject was a p olygon. In such a case, eachevaluation to ok time linear in the numb er of the
p olygon vertices see [28] for the algorithm . For instance, evaluating given by 17 for a
7-gon to ok 0.183s and evaluating its partial derivatives to ok 1.118s; while evaluating and
its partial derivatives for a triangle to ok only 0.067s and 0.412s, resp ectively.
15
The latter shap es are for the initial p ose observer only.
16
The p olygons were constructed by taking random walks on the arrangement of a large numb er of random
lines precomputed by a top ological sweeping algorithm [10]. 28
6.1 On the GHO Observer
We simulated two versions of the GHO observer for the disk-p olygon system: 32 in the
case of frictionless contact =0 between the disk and the p olygon and 43 in the case
c
of rolling contact. The rst version has four state variables: the disk contact u, the edge
contact s which determines the p olygon's p ose , the p olygon's angular velo city ! , and the
tangential comp onent of the p olygon's velo city v . The second version has only three: u,
T
s, ! . The case of sliding contact was not simulated mainly b ecause it is very similar to the
case of frictionless contact except its nonlinear system is more complicated.
The magnitude of the control parameter of the GHO observer directly a ects its p er-
formance. When is to o small, the observer would either converge its estimate to the real
state very slowly or not converge at all. In this case, the error correction would b e dominated
by the original system's drift eld such that it may not be enough to drive the estimate to
some neighb orho o d of the real state where it can converge. On the other hand, when is to o
large, the error correction would dominate the original system, causing the state estimate to
change dramatically and often to diverge. Based on numerous trials, wechose =10inour
simulations.
The disk radius was normalized to 1cm in all simulations. All time measurements will
refer to how long the pushing would have taken place in the real world rather than how long
17
the computation to ok.
To get an idea of the observer's b ehavior, let us lo ok at a simple example of a 7-gon
b eing pushed by the unit disk and rolling on its b oundary see Figure 9 . The tra jectories
of u, s, ! and their estimates u~ , s~, !~ are show in Figure 10. Since the disk contact u is
also the output, its estimate u~ converges faster than the estimates of other state variables.
However, this had caused the following problem in many other instances we simulated: The
feedback u~ u that drives the observer's error corrective term, would usually diminish fast
and b ecome ine ective b efore other estimates can be corrected. To remedy this problem,
our observer turns o error correction in the last 0.04s of every 0.1s interval of pushing so
that the error u~ u would accumulate a bit for the corrective term to b ecome e ective again
at the start of the next 0.1s interval. This scheme has turned out to be quite e ective at
driving other state variable estimates toward convergence.
We rst conducted tests assuming known contact edges. In each test, a state and an
18
estimate were randomly generated over the ranges of the state variables. The test would
be regarded as a success as so on as the di erence between the state and its estimate had
19
b ecome negligible for a p erio d of time; it would b e regarded as a failure if one of the state
variables had gone out of its range rep eatedly or there was no success after a long p erio d of
17
Simulating 0.8s observation of a pushed quadrilateral with rolling contact to ok 232s, while simulating
0.66s observation of the same quadrilateral with frictionless contact to ok 1012s.
18
The range of u in terms of the p olar angle with resp ect to the disk center was set to b e the interval [80; 100]
degrees ; the range of s was determined from the contact edge; the ranges of ! and v were set as [ 1; 1]
T
2
rad=s and [ 0:4; 0:4] cm=s , which were based on the velo city range of the Adept rob ot and on our
simulation data of pushing.
19
The length of the p erio d can b e arbitrarily set but should b e large enough. It was chosen as 0.2s in our
simulations. 29
a Actual Scene b Perceived Scene
Figure 9: A disk of radius 1cm at constantvelo city 5cm=s pushing a 7-gon while observing its
p ose and motion. The snapshots are taken every 0.1s. Contact friction b etween the p olygon
and the disk is assumed to be large enough to allow only the rol ling on the disk edge. The
edge of the p olygon in contact is assumed to be known. The co ecient of supp ort friction
is 0.3. a The scene of pushing for 0.71s. b The imaginary scene as \p erceived" by the
observer 43 during the same time p erio d. The observer constantly adjusts its estimates of
the p olygon's p ose and motion based on the moving contact on the disk b oundary until they
converge to the real p ose and motion. Although the real contact and its estimate were ab out
4.5cm apart on the contact edge at the start of estimation, the error b ecomes negligible in
0.56s. 30
c d
e
Figure 10: State variable tra jectories vs. state estimate tra jectories for the example shown
in Figure 9. The sampling rate is 100Hz. Variable u gives the p olar angle scaled by
the disk radius 1cm of the contact from the disk center. Variable s measures the signed
distance from the contact p oint to the intersection of the contact edge with its p erp endicular
through the p olygon's center of geometry; it has the range [ 5:82; 2:90] cm . Variable !
is the p olygon's angular velo city. These three state variables have estimates u~; s~, and !~ ,
resp ectively. c The tra jectories of u and u~ . d the tra jectories of s and s~; and e the
tra jectories of ! and !~ . Note that u~ and !~ converge faster than s~. 31
Typ e of a No. of Successes
F
2
Pushing cm =s Tests No. Ratio Avg. Time s
Frictionless 0 50 42 84 0.776
Contact 2.5 50 44 88 0.738
0 500 457 91.4 0.965
Rolling 2.5 500 456 91.2 0.939
Table 1: Simulations on observing the p oses and motions of random p olygons b eing pushed
by the unit disk, assuming the contact edges were known. Twoversions of the GHO observers
were simulated: 32 for the case of no friction between a p olygon and the unit disk; and
43 for the case where the p olygon is rolling on the disk b oundary.
20
observation.
Table 1 summarizes the results with known contact edges. There are four groups of
data, each representing a di erent combination of contact mo de and disk acceleration. As
the table indicates, the nger acceleration a did not a ect the observer's p erformance.
F
This seems to be in contradiction with our resort to the use of the normal input eld g ,
N
driven by the normal acceleration a , in the pro of of Theorem 3 on lo cal observability.
F
N
Nevertheless, the use of g serves to simplify the construction of an algebraic pro of of the
N
observability rank condition. We mighthave used the drift eld f only in the pro of, except
the rank condition would b e very hard or even imp ossible to establish.
Figure 11 a shows a simulation example in which a 5-gon making frictionless contact
with the unit disk translating at constantvelo city. Figure 11 b plots the \p olygon motion"
as understo o d by the observer from the contact motion along the disk b oundary. In 0.6s
real time , the observer is able to lo cate the contact p oint thereby determining the p ose
of the 5-gon as well as to estimate its velo city and angular velo city. The tra jectories of the
state variables u, s, ! , v paired with the tra jectories of their estimates u~, s~, !~ , v~ are show
T T
in Figure 12 c , d , e , f , resp ectively.
Since in every test the estimated contact p oint, given by s, was randomly chosen on the
contact edge, it could b e far from the real contact p oint. Yet, the results in Table 1 seem to
suggest that the lo cal GHO observer has \globalness", at least within one edge.
We also observed that the disk contact estimate u~ and the angular velo city estimate !~
always converged very fast, and the tangential velo city estimatev ~ almost always converged.
T
The p ose estimate s~,however, was always part of the divergence whenever it o ccurred. This
phenomenon agrees with our previous discussion following the pro of of Theorem 3 on the
relative \hardness" of observing di erent state variables of the disk-p olygon system.
In the real situation, only the nger contact u is known. In other words, the contact edge,
20
In the simulations, we set this \long p erio d" as 2s. 32
a Actual Scene b Perceived Scene
Figure 11: A disk of radius 1cm at constant velo city 5cm=s pushing and observing a 5-gon.
The contact between the disk and the p olygon is assumed to be frictionless. a The scene
of pushing for 0.6s. b The imaginary scene as \p erceived" by the observer 32 . The real
contact and its estimate were ab out 7.84cm apart on the edge at the start of estimation. The
error b ecame negligible in ab out 0.5s. Figure 12 details the convergence of the estimates of
the p ose, velo city, and angular velo cityofthe p olygon during the push.
the contact lo cation s on the edge, and the velo cities ! and v are all unknown. Accordingly,
T
we mo dify the observer as follows. The observer generates for each edge of the p olygon b eing
pushed a state estimate that hyp othesizes the edge as in contact. Then it simulates the push
21
starting with these estimates in parallel for a short p erio d of time. Assuming that the
estimate hyp othesizing the correct contact edge will likely have converged to the real state
by now, the observer then turns o its error correction and continues the simulation of
the remaining p ossible state tra jectories. The estimate is chosen from the tra jectory that
outlasts all the others in having its u~ stay negligibly close to the the observed disk contact
u. The observer fails if all estimates have gone out of their ranges in the rst p erio d, or the
obtained contact estimate, including the edge and the lo cation on the edge, is incorrect.
Table 2 shows the test results with unknown contact edges. A high p ercentage of the
failures rep orted in the table were due to incorrect contact edges. To explain this, recall
in the disk-p olygon system 24 that the only parameters re ecting the contact geometry
are the distance or height h from the contact edge to the p olygon's centroid and the
signed distance s from the contact to where the edge intersects its p erp endicular from this
center. Contact p oints on di erent edges of approximately the same height and with
approximately the same s can thus result in approximately the same b ehavior of the
disk-p olygon system. Finding a wrong contact edge is therefore exp ected to happ en often
when the p olygon's centroid is approximately equidistant to the real contact edge and to
another edge. In fact, the failures due to incorrect contact edges that we had observed
21
This length of this p erio d was based on the average convergence time in Table 1. 33
c d
e f
Figure 12: State variable tra jectories vs. state estimate tra jectories for the example shown in
Figure 11. Variables u, s, and ! are as sp eci ed in Figure 10. Variable v is the pro jection of
T
the velo cityof P onto the contact tangent. Variable s has the range [ 5:82; 2:90] cm . The
four state variable estimates are u;~ s~, !~ , and v~ , resp ectively. Note that u~ , !~ , v~ converge
T T
faster than s~. 34
Typ e of a No. of Successes Failures
F
2
Pushing cm =s Tests No. Ratio Time s Contact Edge Divergence
Frictionless 0 50 26 52 0.733 9 8 7
Contact 2.5 50 26 52 0.760 9 9 6
0 500 294 58.8 0.893 32 122 52
Rolling 2.5 500 287 57.4 0.891 35 129 49
Table 2: Simulations of the GHO observers 32 and 43 on nding the p oses and motions
of random p olygons b eing pushed by a unit disk, assuming unknown contact edges. There
were three typ es of observation failures, shown from left to right in the three columns under
the \Failures" title bar: 1 the observer found the correct contact edge but not the correct
contact p oint; 2 the observer found the incorrect contact edge; 3 the observer diverged
on all initial estimates.
individually were predominantly of this typ e.
6.2 On the IP Observer
Simulations were conducted for three typ es of pushing: ellipse nger -ellipse ob ject , line-
ellipse, and ellipse-p olygon. No contact friction was assumed in these simulations.
Closed forms of integral exist for p olygons but not for ellipses. On a Sparcstation 20,
one evaluation of takes ab out 2s for an ellipse. The computation of initial accelerations as
in Section 2.3 takes ab out 1.6s for a hexagon and 25s for an ellipse.
During a push, the initial, the nal, and one intermediate contact p ositions on the nger
were recorded, along with the times when the contact reached these p ositions. The initial
p ose observer in Section 4.2 computed p ossible resting p oses of the ob ject which, under the
push, would cause the contact to move to the intermediate p osition on the ngertip at the
recorded time. More sp eci cally, the algorithm guessed a number of initial contacts on the
22
ob ject, and called the Newton-Raphson routine. The nal contact p osition was then used
to further eliminate infeasible p oses.
Table 3 shows the test results under no supp ort friction. These results supp ort our con-
jecture in Section 4.2 that the ob ject p ose can often b e determined from three instantaneous
contacts on the nger during a push.
The slow numerical evaluation of integral prohibits us from conducting large number
of tests on elliptic ob jects under supp ort friction. Simulations under friction were only
p erformed on p olygons, for which closed forms of exist. The 105 tests to ok ab out 65
hours, yielding 94 successes, 11 failures and ambiguities.
22
In the exp eriments, 10 guesses were taken for an ellipse and 3 guesses for each edge of a p olygon. 35
Finger Ob ject No. of Successes
Tests No. Ratio
ellipse ellipse 1000 978 97.8
line ellipse 1000 975 97.5
ellipse p oly 200 189 94.5
Table 3: Simulations of the IP observer with the frictionless plane.
Figure 13: Exp erimental setup of p ose-from-pushing. The co ecient of contact friction
between the part and the disk nger was small measured to b e 0.213 .
6.3 Preliminary Exp eriments
Later we conducted some exp eriments with an Adept 550 rob ot. The \ nger" in our exp er-
iments was a plastic disc held by the rob ot gripp er. The disk edge was marked with angles
from the disk center so a contact p osition could be read by esh eyes. Plastic p olygonal
parts of di erent material were used as ob jects. A plywo o d surface served as the supp orting
plane for pushing. Figure 13 shows the exp erimental setup.
Simulation and exp erimental results on pushing were found to agree closely Figure 14 ,
with slight discrepancies mainly due to shap e uncertainties and non-uniform prop erties of
the disk, the parts, and the plywo o d, all handmade.
We also did some exp eriments on sensing. Instead of one push, two consecutive pushes
were p erformed so that the contact p osition after the rst push served as the intermediate
contact p osition. 36 u1 (deg) 110
100
90 4 8 12162024283236s (cm) 80 0
70 Simulations
u1 (deg) 110
100
90 4 8 12162024283236 s 0(cm) 80
70
Experiments
Figure 14: Simulations versus exp eriments on a triangular part. The graphs show the nal
sensor contact u as a function of the initial part contact s , which here measured the
1 0
distance from a vertex of the part counterclo ckwise along the b oundary. The part b oundary
was discretized into a nite set of lo cations fs g. For each such s we p erformed a numerical
0 0
simulation and a physical exp eriment. The same disk motion lasting 0.75 seconds was used
in each of these simulations and exp eriments. The initial contact u on the pushing disk was
0
always at 90 degrees from its center. The case where s was 11cm in the physical exp eriment
0
is illustrated by the dotted lines: Four feasible p oses were found by the simulator from
contact p osition u = 105 degrees after the push. These four p oses were later distinguished
1
and the real p ose was thus determined using a second contact p osition u determined after
2
a second identical push by the disk. 37
Figure 15: A force sensor for contact sensing. The sensor is comp osed of a horizontal disk
with diameter 3cm and a cylindrical stainless steel b eam erected vertically on the disk and
attached to the gripp er of an Adept rob ot at the top. Two pairs of 350 strain gauges
are mounted on the upp er end of the b eam where they would be most sensitive to any
force exerted on the disk. They measure the contact force along two orthogonal directions,
resp ectively
6.4 Sensor Implementation
Wehave built a \ nger" with tactile capability using four strain gauges as shown in Figure 15.
The strain gauges are mounted near the top of a vertical stainless steel b eam and connected to
an Omega PC plug-in card to form two Wheatstone half bridges. The lower end of the b eam
is attached a disk which serves as the \ nger". A contact with the disk would result in the
b ending of the b eam, whichwould b e detected by the strain gauges. The comp onents of the
contact force exerted on the disk b oundary along the x and y axes of the disk, resp ectively,
can then b e calculated. When contact friction is small enough, the contact force measured by
the gauges would p oint along the disk normal at the contact, thereby indicating the contact
lo cation on the disk b oundary.
The sensor is sensitive enough to detect force in microstrains with a frequency over
2000 Hz. It rep orts the contact in terms of its p olar angle with resp ect to the disk center.
After calibration, the sensed static contacts in 1000 readings constantly have a mean within
one degree away from the real contact and a standard deviation of less than 0.5 degree. For
the sensor to b e applicable, more work is needed to improve the measurement accuracy and
to deal with dynamic friction. 38
7 Summary
Wehave intro duced a sensing approach based on nonlinear observability theory that makes
use of one- nger tactile information. The approach determines the p ose of a known planar
ob ject by pushing it with a nger while \feeling" the contact motion on the ngertip. It also
estimates the ob ject motion during the pushing. Both the nger and the ob ject are assumed
to have piecewise smo oth b oundaries.
We derived a system of nonlinear di erential equations that govern contact and ob ject
motions from geometric and velo city constraints, as well as from the dynamics of pushing.
The state of this pushing system includes the p ose and motion of the ob ject while its output
is the moving contact p osition on the ngertip. We established the lo cal observability of
the system for the case of a disk pushing a p olygon, a result that can generalize to many
other nger and ob ject shap es. This result forms the underlying principle of our sensing
algorithms, which essentially are observers of the nonlinear dynamical system for pushing.
Based on the result of [15], we constructed an asymptotic nonlinear observer and demon-
strated it by simulations. This observer is a comp osition of a copy of the original system with
an error corrective term constructed over the system output and the solution of a Lyapunov-
like equation. It is capable of asymptotically correcting any lo cal error in the estimation of
the ob ject p ose and motion. The observer accepts a sequence of ngertip contacts b eginning
any time during a push. Such an on-line prop erty makes the observer quite exible but it
also requires the sensor to continually provide contact data.
We also presented a nonlinear observer based on Newton's metho d. It determines the
initial resting p ose of the ob ject and thus any p ose from then on from two or three inter-
mediate contact p ositions on the ngertip. In constructing this observer, we viewed pushing
as a mapping from the one-dimensional set of initial motionless ob ject p oses to the set of
one-dimensional contacts on the ngertip at a time instant; and sensing just as its inverse
mapping. Simulations and preliminary exp eriments show that the initial, nal, and another
intermediate contact p ositions usually suce for p ose determination. Although this observer
requires much less sensor data than the rst observer, it is more likely to b e a ected by sen-
sor noises and uncertainties in ob ject motion due to p ossible impact b etween the nger and
the ob ject at the b eginning of pushing.
Although in certain worst cases global sensing ambiguities can never b e eliminated with
one push, a sensing failure may be removed by pushing rep eatedly at di erent sections of
the ob ject b oundary.
Both supp ort friction in the plane and contact friction b etween the ob ject and the nger
have b een taken into account.
Another implementation, for the initial p ose observer only,mayemb ed three p oint light
detectors in the ngertip. Keep the initial contact always at the rst light detector so that
it is covered. Di erent initial ob ject p oses would result in di erent times when the second
and third light detectors are covered. To enhance eciency, we could simply discretize the
b oundary of a part and precompile a table from which p oses can b e directly lo oked up with
the recorded time instants.
Contact motion is the sub ject of our pap er for it enco des a range of information that we
want to know ab out a task. It is generated by pushing and deco ded with the application 39
of nonlinear observability theory. It is clear that our metho d will be applicable in other
situations where contact motion or similar tactile information exists regardless of the typ e
of manipulation that generates such motion.
From a more general p ersp ective, the information for accomplishing a manipulation task
may exist in various forms, implicitly or explicitly. Accordingly, there may be various ways
of obtaining such information and making use of it. This pap er uses the example of contact
motion and applies nonlinear observability theory to illustrate the ab ove idea. A good
strategy for achieving a task usually comes from an understanding of its geometry and
mechanics.
We view a sensing algorithm as the part of a sensing strategy that interprets data acquired
by physical sensors to derive information essential for a task. It can be a computational
algorithm based on task geometry,a control system based on task mechanics, or something
else. Just as geometric sensing resorts to computational geometry algorithms, the sensing
strategy presented in this pap er applies nonlinear control techniques. In devising a sensing
strategy wehave to decide what p ortion of the sensing is to b e done byphysical sensors and
what p ortion to be carried out by the sensing algorithm. A good strategy should exploit
to some extent the sensors available as well as the task geometry and mechanics. Here we
would like to refer the reader to Erdmann's metho dology [11] on task-sp eci c and action-
based sensor design.
Acknowledgements
Supp ort for this research was provided in part by Carnegie Mellon University, and in part
by the National Science Foundation through the following grants: NSF Presidential Young
Investigator award IRI-9157643 and NSF Grant IRI-9213993.
Thanks to Matt Mason for having interesting discussions on the topics of pushing and
pressure distribution, for suggesting the use of force sensing in real implementation, and
for p ointing us to nonlinear observability theory; to Ken Salisbury, Alan Guisewite, and
Ralph Hollis for their feedback on the exp erimental setup, and esp ecially, on the use of
strain gauges; to Garth Zeglin for generous help with the exp eriments; to Kevin Lynch for
intro ducing us to his related work on active sensing by pushing using tactile feedback, and
for o ering many helpful comments on the pap er; to Wes Huang for realizing the connection
of our sensing work to observer design; and to Alfred Rizzi for an interesting discussion on
nonlinear observers.
Many thanks to the anonymous reviewers whose insightful and constructive comments
have help ed strengthen the technical soundness as well as improve the intuitive illustration
of the pap er. Sections 1, 2, 4.2, 6.2, and 6.3 are based on Jia and Erdmann [30] presented at
the 1996 IEEE International Conference on Rob otics and Automation, while sections 3, 4.1,
6.1, and 6.4 are based on Jia and Erdmann [31] presented at the 1998 IEEE ICRA. Thanks
to the anonymous reviewers of these two conference pap ers for their valuable comments. 40
References
[1] Srinivas Akella and Matthew T. Mason. Posing p olygonal ob jects in the plane by
pushing. In Proceedings of the 1992 IEEE International Conference on Robotics and
Automation, pages 2255{2262, 1992.
[2] J. C. Alexander and J. H. Maddo cks. Bounds on the friction-dominated motion of a
pushed ob ject. International Journal of Robotics Research, 12 3 :231{248, 1993.
[3] P. K. Allen and K. S. Rob erts. Haptic ob ject recognition using a multi- ngered dex-
trous hand. In Proceedings of the 1989 IEEE International ConferenceonRobotics and
Automation, pages 342{347, 1989.
[4] K.-F. Bohringer, B. R. Donald, R. Mihailovich, and N. C. MacDonald. Sensorless
manipulation using massively parallel microfabricated actuator arrays. In Proceedings
of the 1994 IEEE International ConferenceonRobotics and Automation, pages 826{833,
1994.
[5] Randy C. Brost and Kenneth Y. Goldb erg. A complete algorithm for synthesizing
mo dular xtures for p olygonal parts. In Proceedings of the 1994 IEEE International
Conference on Robotics and Automation, pages 535{542, 1994.
[6] Chunsheng Cai and Bernard Roth. On the spatial motion of a rigid body with p oint
contact. In Proceedings of the 1987 IEEE International Conference on Robotics and
Automation, pages 686{695, 1987.
[7] Alec Cameron, Ron Daniel, and Hugh Durrant-Whyte. Touch and motion. In Proceed-
ings of the 1988 IEEE International Conference on Robotics and Automation, pages
1062{1067, 1988.
[8] G. Ciccarella, M. Dalla Mora, and A. Germani. A Luenb erger-like observer for nonlinear
systems. International Journal of Control, 57 3 :537{556, 1993.
[9] P. E. Crouch. Dynamical realizations of nite Volterra series. SIAM Journal on Control
and Optimization, 19 1 :177{202, 1981.
[10] Herb ert Edelsbrunner and Leonidas J. Guibas. Top ologically sweeping an arrangement.
In Proceedings of the 18th Annual ACM Symposium on Theory of Computing, pages
389{403. ACM Press, 1986. Berkeley, CA.
[11] Michael Erdmann. Understanding action and sensing by designing action-based sensors.
International Journal of Robotics Research, 14 5 :483{509, 1995.
[12] Michael Erdmann and Matt Mason. An exploration of sensorless manipulation. IEEE
Journal of Robotics and Automation, 4 4 :369{379, 1988.
[13] R. S. Fearing and T. O. Binford. Using a cylindrical tactile sensor for determining
curvature. In Proceedings of the 1988 IEEE International Conference on Robotics and
Automation, pages 765{771, 1988. 41
[14] Peter C. Gaston and Tom as Lozano-P erez. Tactile recognition and lo calization using
ob ject mo dels: The case of p olyhedra on a plane. IEEE Transactions on Pattern
Analysis and Machine Intel ligence, 6 3 :257{266, 1984.
[15] J. P. Gauthier, H. Hammouri, and S. Othman. A simple observer for nonlinear systems
applications to bioreactors. IEEE Transactions on Automatic Control, 37 6 :875{880,
1992.
[16] J. P. Gauthier and I. A. K. Kupka. Observability and observers for nonlinear systems.
SIAM Journal on Control and Optimization, 32 4 :975{994, 1994.
[17] Kenneth Y. Goldb erg. Orienting p olygonal parts without sensors. Algorithmica,
8 10 :201{225, 1993.
[18] Lawence E. Go o dman and William H. Warner. Statics and Dynamics. Wadsworth
Publishing Company, Inc., Belmont, CA, 1964.
[19] Suresh Goyal, Andy Ruina, and Jim Papadop oulos. Planar sliding with dry friction.
Wear, 143:307{352, 1991.
[20] W. E. L. Grimson. Object Recognition by Computer: the Role of Geometric Constraints.
MIT Press, Cambridge, 1990.
[21] W. E. L. Grimson and T. Lozano-P erez. Mo del-based recognition and lo calization from
sparse range or tactile data. International Journal of Robotics Research, 3 3 :3{35,
1984.
[22] David D. Grossman and Michael W. Blasgen. Orienting mechanical parts by computer-
controlled manipulator. IEEE Transaction on Systems, Man, and Cybernetics, 5 5 :561{
565, 1975.
[23] E. J. Haug, S. C. Wu, and S. M. Yang. Dynamic mechanical systems with coulomb
friction, stiction, impact and constraint addition-deletion|I: Theory. Mechanisms and
Machine Theory, 21 5 :407{416, 1986.
[24] Rob ert Hermann and Arthur J. Krener. Nonlinear controllability and observability.
IEEE Transactions on Automatic Control,AC-22 5 :728{740, 1977.
[25] Rob ert D. Howe and Mark R. Cutkosky. Dynamic tactile sensing: Perception of ne sur-
face features with stress rate sensing. IEEE Transactions on Robotics and Automation,
9 2 :140{151, 1993.
[26] Rob ert D. Howe and Mark R. Cutkosky. Practical force-motion mo dels for sliding
manipulation. International Journal of Robotics Research, 15 6 :557{572, 1996.
[27] A. Isidori. Nonlinear Control Systems: An Introduction. Springer-Verlag, 1985. 42
[28] Yan-Bin Jia. Geometric and Dynamic Sensing: Observation of Pose and Motion through
Contact. PhD thesis, Carnegie Mellon University, The Rob otics Institute, 1997. CMU-
RI-TR-97-50.
[29] Yan-Bin Jia and Michael Erdmann. Geometric sensing of known planar shap es. Inter-
national Journal of Robotics Research, 15 4 :365{392, 1996.
[30] Yan-Bin Jia and Michael Erdmann. Pose from pushing. In Proceedings of the 1996
IEEE International Conference on Robotics and Automation, pages 165{171, 1996.
[31] Yan-Bin Jia and Michael Erdmann. Observing p ose and motion through contact. In
Proceedings of the 1998 IEEE International Conference on Robotics and Automation,
1998.
[32] Arthur J. Krener and Alb erto Isidori. Linearization by output injection and nonlinear
observers. Systems Control Letters, 3:47{52, 1983.
[33] Zexiang Li and John Canny. Motion of two rigid b o dies with rolling constraint. IEEE
Transactions on Robotics and Automation, 6 1 :62{72, 1990.
[34] David G. Luenb erger. An intro duction to observers. IEEE Transactions on Automatic
Control,AC-16 6 , 1971.
[35] Kevin Lynch, Hitoshi Maekawa, and Kazuo Tanie. Manipulation and active sensing
by pushing using tactile feedback. In Proceedings of the 1992 IEEE/RSJ International
Conference on Intel ligent Robots and Systems, pages 416{421, 1992.
[36] William Duncan MacMillan. Dynamics of Rigid Bodies. McGraw-Hill Inc., 1936. Dover
reprint, 1960.
[37] Matthew T. Mason. Mechanics and planning of manipulator pushing op erations. In-
ternational Journal of Robotics Research, 5 3 :53{71, 1986.
[38] David J. Montana. The kinematics of contact and grasp. International Journal of
Robotics Research, 7 3 :17{32, 1988.
[39] Henk Nijmeijer and Arjan van der Schaft. Nonlinear Dynamical Control Systems.
Springer-Verlag, New York, 1990.
[40] Jong-Shi Pang and Je rey C. Trinkle. Complementary formulations and existence of
solutions of dynamic multi-rigid-b o dy contact problems with Coulomb friction. Math-
ematical Programming, 73:199{226, 1996.
[41] Eric Paulos and John Canny. Fast construction of near optimal probing strategies. In
Jean-Paul Laumond and Mark Overmars, editors, Algorithms for Robotics Motion and
Manipulation, pages 185{195. A. K. Peters, 1997. Pro ceedings of the 1996 Workshop
on the Algorithmic Foundations of Rob otics. 43
[42] M. A. Peshkin and K. Goldb erg. Curved fences for part alignment. In Proceedings of
the 1993 IEEE International Conference on Robotics and Automation, pages 467{573,
1993.
[43] Elon Rimon and Andrew Blake. Caging 2D b o dies by 1-parameter two- ngered gripping
systems. In Proceedings of the 1996 IEEE International Conference on Robotics and
Automation, pages 1458{1464, 1996.
[44] Kenneth Salisbury. Interpretation of contact geometries from force measurements. In
M. Brady and R. Paul, editors, Robotics Research, pages 565{577. The MIT Press, 1984.
[45] Michael Spivak. A Comprehensive Introduction to Di erential Geometry, volume 1.
Publish or Perish, Inc., Boston, 1970.
[46] Gerta Zimmer. State observation by on-line minimization. International Journal of
Control, 60 4 :595{606, 1994. 44