New IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. …ai.stanford.edu/~liugf/grasp-opt.pdf ·...

IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. XX, NO. Y, MONTH 2004 100

On Quality Functions for Grasp Synthesis, FixturePlanning and Coordinated Manipulation

Guanfeng Liu, Jijie Xu, Xin Wang, Zexiang Li

Abstract— Planning a proper set of contact points on a givenobject/workpiece so as to satisfy a certain optimality criterion isa common problem in grasp synthesis for multifingered robotichands and in fixture planning for manufacturing automation. Inthis paper, we formulate the grasp planning problem as opti-mization problems with respect to three grasp quality functions.The physical significance and properties of each quality functionare explained, and computation of the corresponding gradientflows is provided. One noticeable property of some of thesequality functions is that the optimal solutions are also force-closure grasps if they do exist for the given object. Furthermore,when specialized to two-fingered or three-fingered grasps ona spherical object, the optimal solutions become the familiarantipodal grasp, or the symmetric grasp, respectively. Thus, byfollowing the gradient flows with arbitrary initial conditions,the optimal grasp synthesis problem is solved for objects withsmooth geometries manipulated by hands with any number offingers. Also, note that our solutions do not involve linearizationof the friction cones. We discuss two simplified versions of theseproblems when real-time solutions are needed, e.g., coordinatedmanipulation of a robotic hand with contact points servoing. Wegive simulation and experimental results illustrating validity ofthe proposed approach for optimal grasp planning.

Index Terms— Grasp synthesis, Max-transfer problem, Max-normal-grasping-force problem, Min-analytic-center problem,gradient computation.

I. I NTRODUCTION

Planning a proper set of contact points on a given ob-ject/workpiece so as to satisfy a certain optimality criterion is acommon problem in grasp synthesis for multifingered robotichands, and in fixture planning for manufacturing automation.During a full multifingered manipulation cycle, grasp planningarises in several occasions, such as when an object is firstpicked up from say, a table top; or when the object ismanipulated from an initial to a final grasp configurationthrough a continuum of force-closure grasps in order to notdropping the object (dextrous manipulation); or when theobject is coordinatively manipulated to execute a given task(e.g., scribing) with contact points servoing (so as to maintainthe object in an optimal grasp configuration). Research ongrasp planning centers on two broad categories:grasp analysisandgrasp synthesis.

Early work on grasp analysis includes that of Reulaux [1],who introduced the notion of force-closure and form-closuregrasps; that of Salisbury [2], who developed mathematical

This project is supported by RGC Grant No. HKUST 6187/01E, HKUST6221/99E, CRC98/01.EG02, and NSF(50029501) of P.R.C.

The authors are with Dept. of Electrical and Electronic Engineering, HongKong University of Science and Technology,Clear Water Bay, Kowloon,Hong Kong (e-mail: [email protected]; [email protected]; [email protected];[email protected]; fax (852)2358-1485)

models of contact and grasp, and provided necessary andsufficient conditions for force-closure grasps; that of Mishraet al. [3] for frictionless point contacts, who showed thata grasp is force closure if and only if the origin of thewrench space lies in the interior of the convex hull of theprimitive wrenches. Several force closure tests based on theseconditions were developed by Chen and Burdick [4], Nguyen[5] and Trinkle [6]. Bicchi [7] translated the force-closureproblem into the stability of an ordinary differential equation.Recently, by linearizing the friction cones, Liu [8] introduceda ray-shooting problem (LP) and proposed a clean-cut test forforce-closure grasps. Han, Trinkle, and Li[9] observed that thenonlinear friction cone constraints can be represented as LinearMatrix Inequalities (LMIs) and the force-closure problemcan be reformulated as the feasibility problem of a semi-definite or max-det problem, for which efficient algorithmsare now available. Thus, the general problem of determiningif a grasp is force closure is considered to be completelysolved. Furthermore, the problem of computing optimal fingerforces within the limits of the friction cones to balance a givenexternal wrench is also solved with the work of [10], [11], [9].

Research on optimal grasp synthesis consists of: (a) deter-mination of optimality criteria; and (b) derivation of methodsand algorithms for computing contact locations with respect tothe optimality criteria and subject to accessibility constraints.Early work in this area includes synthesis of grasps forpolygonal and polyhedral objects which are force closure. Jiand Roth [12] derived conditions on contact positions andsurface normals that guarantee a grasp to be force closure.Nguyen [5] gave conditions for constructing planar two-fingered force closure grasps, which was generalized by Ponceand Faverjon [13] to three-fingered case, and by Ponce etal. [14] to four-fingered case. Mishra et al. [3] proposed analgorithm for computing force-closure grasps for polyhedralobjects under frictionless point contacts. Ding et al. [15]proposed heuristics for searching an eligible set of graspingsurfaces of a polyhedra and a quadratic programming approachfor selecting an optimal form closure grasp that minimizesthe positioning errors. Liu [16] proposed an algorithm forcomputing all form closure grasps of polygonal objects witharbitrary number of fingers. Apparently, to a given object thereexist in general a large set of grasps which are force closure.In other words, force closure is too coarse a criterion to beused for grasp synthesis. More refined criteria are neededto define the notion of grasp optimality. Cutkosky [17] andLi and Sastry [18] proposed the use of task requirement forgrasp selection. For general two-fingered grasps, Hong et al.[19] used the distance between two fingers as a grasp quality


function, of which antipodal grasps are the optimal solutions.Using this function, Chen and Burdick [4] developed gradientalgorithms for grasp planning. Similar works could also befound in [20], [21], [22]. A great deal of difficulties exist whenone aims to extend this approach to grasps with more thantwo fingers except the particular case, a three-fingered handgrasping a spherical object, for which the area of the triangleformed by the three contact points is used as a physicallymeaningful quality function. The optimal solution for thisfunction turns out to be the symmetric grasp where the threefingers locate at three symmetric points of a big circle. Todevelop a general approach to grasp synthesis that is notconfined to objects with specific geometries, Kirkpatrick et al.[23] proposed a quality measure based on the capability of thegrasp in resisting external wrenches. They further translatedthe problem to the computation of the radius of the largestL2

ball contained in the convex hull of the primitive wrenches.The same idea was also adopted by Ferrari and Canny [24]. Toavoid the ambiguity arising in defining physically meaningfulnorms for external wrenches, Mirtich and Canny [25] proposedtwo quality functions via decoupling the force and momentcomponents of a wrench. Based on these two functions, theycomputed several examples and obtained the well knownoptimal grasps by other approaches. Zhu et al. [26], [27]introduced theQ distance and adopted the radius of the largestQ ball contained in the convex hull of the primitive wrenchesas a quality measure.

To summarize, a complete solution to the general optimalgrasp synthesis problem rests on derivation of grasp qualityfunctions which: (1) incorporate the force closure condition,i.e., optimal solutions are also force-closure grasps; and (2)have easily computable gradients. In other words, an optimalgrasp can be attained by following the gradient flows of thequality functions starting from some initial conditions whichmay not be force closure. Based on our review of previousworks, this problem remains largely unsolved. The aim of thispaper is to develop solutions to this problem that: (i) have cleansenses of optimality; (ii) do not involve approximation of thefriction cones; and (iii) can be applied to objects with smoothgeometries grasped by hands with any number of fingers.

First, we will introduce several candidate grasp qualityfunctions and formulate the grasp synthesis problem as a Max-transfer, a Max-normal-grasping-force, and a Min-analytic-center problem. The physical meaning of each quality func-tion will be explained. Each problem will assume the formof max−min−max or min−max−min type. Then, wewill develop algorithms for computing the gradients of thesequality functions. When real-time solutions are needed forapplications such as contact points servoing in coordinatedmanipulation [28], [20], we introduce two simplified qualityfunctions, along with several examples. Note that the optimalsolutions of the simplified problems coincide with previousresults obtained using heuristic approaches, demonstratingagain generality of our current methods. Finally, we performexperimental studies on the HKUST three-fingered hand usingreal-time optimization of the simplified quality functions.

The paper is organized as follows. In Section II, we brieflyreview the kinematic model of a multifingered hand manip-

Fig. 1. A k-fingered hand grasping an object

ulation system and the friction cone constraints. In SectionIII, we discuss several classical grasping examples and theiroptimal solutions by heuristic approaches. In Section IV, weshow how to compute the gradients formax−min−max andmin−max−min problems and propose numerical algorithmsfor grasp planning. In Section V, we introduce three newcandidate quality functions for grasp synthesis, along withsimulation results of a three-fingered hand grasping an ellip-soid. In Section VI, we derive two simplified quality functionsfor real-time grasp planning. Several examples are studiedshowing that the optimal solutions of the simplified problemscoincide with those using heuristic approaches. In Section VII,we perform experimental studies on the HKUST three-fingeredhand with real-time optimization of the simplified functions.In Section VIII, we end this paper with a short discussion offuture work.

II. GRASPMODELS AND FRICTION CONSTRAINTS

In this section, we review the kinematic model of a mul-tifingered hand manipulation system and the friction coneconstraints.

A. Grasp Models

Consider ak-fingered hand grasping an object as shownin Fig. 1. Assume that all fingers make contacts of constanttypes with the object. Three contact models, frictionless pointcontact (FPC), point contact with friction (PCWF), and softfinger contact with elliptic approximation (SFCE) are consid-ered in our analysis. Following the notations in [29][30], weattach an object frameO to the center of mass of the object,finger frameFi (i = 1, · · · , k) to the fingertip of theith

finger, and local framesLoi and Lfi (i = 1, · · · , k) to theobject and fingeri, respectively, at the point of contact. Aconfiguration of contact is described by contact coordinatesηi = (αT

oi, αTfi, ψi)T ∈ R5, whereαoi = (uoi, voi)T ∈ R2

are the local coordinates of contact relative to the object,αfi = (ufi, vfi)T ∈ R2 the coordinates of contact relativeto the fingertip, andψi the angle of contact. Collectively,a contact configuration of the system is described in local


coordinates byη = (ηT1 , · · · , ηT

k )T ∈ R5k. In this paper, werepresent a grasp as

~αo = [αTo1, · · · , αT

ok]T ∈ R2k.

The relation between the applied finger forces and the resultingobject wrench is given by the grasp map,G ∈ R6×n,

wo = Gx (1)

wherex = [x1T · · ·xk

T ]T ∈ Rn, with xi ∈ Rni , i = 1, · · · , kand n =

∑ki=1 ni, is the vector of finger forces. The finger

force is constrained to the friction cone

FCi = xi ∈ Rni |xi,n ≥ 0, ‖xi,t‖s ≤ xi,n,or collectively to

FC = FC1 × · · · × FCk = x ∈ Rn|xi ∈ FCi,with xi,n and xi,t being, respectively, the normal and thetangential components of the finger forces at theith point ofcontact. Here,xi,n = xi,3 for PCWF and SFCE models andxi,n = xi for FPC.‖xi,t‖s denote vector norms described foreach of the contact models by:

FPC :‖xi,t‖s = 0 (2)

PCWF :‖xi,t‖s =1µi

√xi,1

2 + xi,22 (3)

SFCE :‖xi,t‖s =

√1µ2

i

(xi,12 + xi,2

2) +1

µ2i,t

xi,42 (4)

with xi,1 andxi,2 being the friction force components in thetangential plane,xi,4 the moment along the contact normal,µi the Coulomb friction coefficient, andµi,t the coefficient oftorsional friction.

A grasp(G(~αo),FC) is said to be force closure if and onlyif G(FC) = R6.

B. Friction Cones as Semi-definite Constraints

By refining the results of [10], [11], Helmke, Hueper andMoore [31] showed that the friction cone constraints (3) isequivalent to positive semi-definiteness of the following2× 2symmetric matrix:

Pi =[

µixi,3 + xi,1 xi,2

xi,2 µixi,3 − xi,1

]≥ 0

; (4) is equivalent to

Pi =

[xi,3 + 1

µixi,1

1µi

xi,2 − j 1µi,t

xi,41µi

xi,2 + j 1µi,t

xi,4 xi,3 − 1µi

xi,1

]≥ 0,

where j =√−1; and the friction constraints of the hand is

equivalent to

P ∈ RN×N = diag(P1, · · · , Pk) ≥ 0 , N = 2k. (5)

Han, Trinkle and Li [9] further observed that (5) can bereformulated as Linear Matrix Inequalities (LMIs) of the form

P = A1x1 + · · ·+ Anxn ≥ 0,

with a reordering of the finger force indices. The force balanceequation (1) is also translated into

Tr(BiP ) = wo,i , i = 1, · · · , 6, (6)

whereBi = BTi ∈ RN×N are coefficient matrices.

Fig. 2. A 2-finger antipodal grasp

III. G RASPPLANNING : REVIEW OF CLASSICAL

EXAMPLES AND HEURISTIC APPROACHES

In this section, we discuss previous heuristic approachesused in two classical examples of grasp planning.

Let’s briefly review the conditions for a2-fingered force-closure grasp and that of a2-fingered antipodal grasp. Weassume that the object is devoid of holes and has a closedsurface which is homeomorphic toS2. Following the notationof Do Carmo [32], we parameterize the surface of the objectby

X(αo) =[

x(αo) y(αo) z(αo)]T

, αo = [uo, vo]T ∈ R2.

Xuo , Xvo andn(αo) = Xuo×Xvo

‖Xuo×Xvo‖ are, respectively, two tan-gent vectors and the outward normal vector atαo = (uo, vo)T .It is well known [19], [33], [4], [5] that a2-fingered graspwith contact pointsαo1 = [uo1, vo1]T and αo2 = [uo2, vo2]T

is force closure if and only if

n(αo1) · X(αo1)−X(αo2)‖X(αo1)−X(αo2)‖ > cf1

n(αo2) · X(αo2)−X(αo1)‖X(αo2)−X(αo1)‖ > cf2

for squeezing grasps

n(αo1) · X(αo1)−X(αo2)‖X(αo1)−X(αo2)‖ < −cf1

n(αo2) · X(αo2)−X(αo1)‖X(αo2)−X(αo1)‖ < −cf2

for expanding grasps. Here,cfi = cos(tan−1 µi), i = 1, 2.In general,2-fingered force closure grasps are not uniqueand force closure regions can be identified for polygonalobjects [5] and curved 2D objects [34]. An important problemnaturally arises as which grasp in this region is the best. Honget al. [19] first introduced the concept of antipodal grasps andproposed the following distance function

E(αo1, αo2) =12‖X(αo1)−X(αo2)‖2 (7)

whose critical points give candidates of antipodal configura-tions. Antipodal grasps are necessarily force-closure graspsand are regarded as the best among all2-fingered grasps,as shown in Fig. 2. Antipodal grasps can be synthesized byplanning the contact points to follow the ascent gradient ofEor other equivalent cost functions [20], [4].


Fig. 3. A 3-finger symmetric grasp

In general, the above heuristic approach can not be extendedto 3-fingered grasps on objects of arbitrary geometries. How-ever, for a spherical object, we can let the square of the areaformed by the three contact points to be the objective function

E(~αo) = 14 (‖X(αo3)−X(αo1)‖2‖X(αo2)−X(αo1)‖2

− ((X(αo3)−X(αo1)) · (X(αo2)−X(αo1)))2)

and the optimal solutions are symmetric grasps with threecontact points located uniformly on a big circle, as shownin Fig 3. To solve generally the grasp synthesis problem, inthe sections follows we will introduce several grasp qualityfunctions which assumes the form ofmin−max−min ormax−min−max, for which gradient algorithms can bedeveloped.

IV. T HEORY AND ALGORITHMS FORmin−max−minAND max−min−max PROBLEMS

In this section, we first review the general the-ory of min−max, max−min, min−max−min, andmax−min−max problems. Then, we propose algorithms forsolving these problems.

A. Theory of Gradient Computation

Consider themin-max problem:

minY

maxZ

F0(Y,Z) , Y ∈ Ω1 ⊂ RN1 , Z ∈ Ω2 ⊂ RN2 (8)

where Ω1 is an open set andΩ2 a bounded closed subset.We shall assume thatF0(Y, Z) and ∂F0(Y,Z)

∂Y is continuous onΩ1 × Ω2. Let

F1(Y ) = maxZ∈Ω2

F0(Y, Z).

F1(Y ) possesses the following properties:

1) F1(Y ) is continuous onΩ1;2) SupposeΩ1 ⊂ Ω1 and for someY0 ∈ Ω1 the set

Y ∈ Ω1 | F1(Y ) ≤ F1(Y0)is bounded. Then, there exists a pointY ∗ ∈ Ω1 suchthat

F1(Y ∗) = infY ∈Ω1F1(Y )

maxZ∈Ω2 F0(Y ∗, Z) = infY ∈Ω1maxZ∈Ω2 F0(Y, Z).

If Ω1 is chosen as a local set, thenY ? is a local minimum,otherwise, it is a global optimum.

It is often impossible to find analytic solutions for themin−max and max−min problems, and thereby those forthemin−max−min andmax−min−max problems. Seek-ing a possible numerical solution requires us to compute thegradients of those quality functions in an efficient way. We firstconsider computation of the gradient of the following problem:

∇Y F1(Y ) =∂F1(Y )

∂Y

For fixedY ∈ Ω1, we define

R(Y ) = Z ∈ Ω2 | F0(Y,Z) = maxZ

F0(Y,Z).Obviously,R(Y ) ⊂ Ω2 is a bounded closed set. The followingtheorem [35] states how to compute the directional derivativeof F1(Y ):

Theorem 1: F1(Y ) is a differentiable function with itsdirectional derivative atY ∈ Ω1 along v ∈ RN1 , ‖v‖ = 1,given by

〈∂F1(Y )∂Y

, v〉 = maxZ∈R(Y )

〈∂F0(Y,Z)∂Y

, v〉.From this theorem, we conclude that

∇Y F1(Y ) =∂F0(Y, Z)

∂Y|Z∗

if Z∗ is the unique optimal solution formaxZ F0(Y,Z). Wecan derive similar results for themax-min problem.

Second, let us consider the followingmin−max−minproblem

minY ∈Ω1

maxZ∈Ω2

F0(Y, Z) = minY ∈Ω1

maxZ∈Ω2

minW∈Ω3

F (Y,Z, W ) (9)

whereΩ3 is an open or close set. GivenY andZ, we assumethat W ∗(Y, Z) is an optimal solution for

minW∈Ω3

F (Y,Z, W ). (10)

Then,F0(Y,Z) = F (Y, Z, W ∗(Y, Z)).Theorem 2: If the following three conditions are satisfied:1) There is a unique solutionW ∗(Y, Z) to (10);2)

∂W ∗(Y, Z)∂Y

= 0; (11)

3) Z∗ is the unique optimal solution for

maxZ∈Ω2

F (Y,Z, W ∗(Y, Z)) = maxZ∈Ω2

minW∈Ω3

F (Y, Z, W ). (12)

Then,

∇Y F1(Y ) =∂F (Y,Z, W )

∂Y|Z∗,W∗(Y,Z∗) . (13)

Proof: SinceZ∗ is the unique optimal solution to (12), wehave from Theorem 1 that

∇Y F1(Y ) =∂F (Y, Z,W ∗)

∂Y|Z∗ +

∂F (Y, Z, W ∗)∂W ∗

∂W ∗

∂Y|Z∗ .

(14)The second term in the right hand side is equal to zero becauseof (11). ¤This approach can also be applied tomax−min−maxproblems satisfying the similar conditions.


B. Numerical Algorithms

In this subsection, we will develop an algorithm for themin-max problem, and an algorithm for themin-max-minproblem.

For problem (8), we assume that

maxZ

F0(Y, Z)

has a unique solution and can be solved using some algo-rithm (called Algorithm A). Then we develop the followingalgorithm for (8):

Algorithm 1: Algorithm for the min-max problem

Input: initial value Y (0), step sizeγk > 0, andtoleranceε > 0;Output: optimal valueY ∗;Step1: setk = 0;Step2: solveZ∗(k) = maxZ F0(Y (k), Z) using Al-gorithm A, and calculateF1(k) = F0(Y (k), Z∗(k));Step3: calculate the gradient

∇Y F1(Y ) |Y (k)=∂F0(Y,Z)

∂Y|Y (k),Z∗(k);

Step4: set

Y (k + 1) = Y (k)− γk∇Y F1(Y ) |Y (k);

Step5 :solve Z∗(k + 1) = maxZ F0(Y (k + 1), Z)using Algorithm A, and calculateF1(k + 1) =F0(Y (k + 1), Z∗(k + 1));Step 6: if |F1(k + 1) − F1(k)| ≤ ε, output Y ∗ =Y (k + 1); else setk = k + 1 and go to Step3.

The Algorithm A used in Step5 depends on the propertiesof the cost functionF0(Y,Z). It could be linear program-ming algorithms, semi-definite programming algorithms, orinterior point algorithms. The uniqueness of the solution formaxZF0(Y,Z) is often satisfied.

To solve themin-max-min problem (9), we assume thatall three conditions in Theorem 2 are satisfied. We design thefollowing algorithm

Algorithm 2: Algorithm for the min-max-min problem

Input: initial value Y (0), step sizeγk > 0, andtoleranceε > 0;Output: optimal valueY ∗;Step1: setk = 0;Step 2: solve Z∗(k) and W ∗(Y (k), Z∗(k)) usingAlgorithm 1 for

maxZ∈Ω2

minW∈Ω3

F (Y (k), Z, W ),

and calculateF1(k) = F0(Y (k), Z∗(k));Step 3: calculate the gradient∇Y F1(Y ) |Y (k) as(13);Step4: set

Y (k + 1) = Y (k)− γk∇Y F1(Y ) |Y (k);

Step5: solveZ∗(k+1) andW ∗(Y (k+1), Z∗(k+1)), and calculateF1(k+1) = F0(Y (k+1), Z∗(k+1))as Step2;

Step 6: if |F1(k + 1) − F1(k)| ≤ ε, output Y ∗ =Y (k + 1); else setk = k + 1 and go to Step3.

Remark 1: In the grasp synthesis problems that will beintroduced in the section follows, condition (1) in Theorem2 is often satisfied. However, condition (2) may not be true.Here we adopt numerical approximation in (14):

∂W ∗(Y, Z)∂Y

=W ∗(Y + δY, Z)−W ∗(Y, Z)

δY.

In general we can only find local optimum in condition (3),which means that our algorithms can only be used to find localoptimum for the min−max−min and max−min−maxproblems. Moreover, the efficiency of the algorithms relies onthe chosen step sizes, please refer to [4] for more details.

V. SEVERAL GRASPQUALITY FUNCTIONS AND

SIMULATION EXAMPLES

In this section, we introduce three new grasp quality func-tions and formulate the corresponding optimal grasp synthesisproblems.

A. The Max-transfer Problem

Grasp map can be regarded as a transfer function takingfinger forces to object wrenches with a domain being thefriction cones. Planning of optimal grasps amounts to findinga set of contact points which optimize, in some sense, thetransfer function.

Kirkpatrick, Mishra and Yap [23] utilized thequantitativeSteinitz’s Theoremto evaluate a grasp, where the radius ofthe largest ball, centered in the origin of the wrench spaceand contained in the convex hull spanned by unit primitiveforces, measures the quality of a grasp. Ferrari and Canny[24] proposed a global grasp quality measure by minimizingthe maximal proportion between the norm of external forcesand that of its respective finger forces. The same idea wasfurther developed by Mirtich and Canny [25], where forceand moment transfer functions were treated independently andoptimized sequentially. By doing so, the ambiguity arisingin specifying a physically meaningful norm for the externalwrench space can be avoided. Since all the problems discussedin these works consider the optimal grasp planning from theability of the system in resisting external wrenches, we callthem the Max-transfer problem.

It is well known thatRn = R(GT ) ⊕ N(G), any fingerforce x can be uniquely decomposed into two components

x = x⊥ + x‖ , x⊥ ∈ R(GT ) , x‖ ∈ N(G)

where x⊥ = GT (GGT )−1Gx can be interpreted as themanipulation force [36], andx‖ = (I −GT (GGT )−1G)x theinternal grasping force. The manipulation force is determinedas long as the external wrench is given. The undeterminedcomponent is the internal grasping force. Considering thatfinger forces with large magnitude are not allowed duringmanipulation, the grasp quality at~αo can be measured asthe minimal proportion (worst case) between the norm of theexternal wrench and that of the finger forces while fixing thenorm of the manipulation force:

x⊥Tx⊥ = xT GT (GGT )−1Gx = wT

o (GGT )−1wo = 1.


0 50 100 150 200 250 300 350

0.2

0.25

0.3

No. of iterations

1/g 1

Trajectory of the cost function

Fig. 4. View 3: Trajectory of the cost function

−1

0

1−1 −0.5 0 0.5 1

−3

−2

−1

0

1

2

3

x

y

Trajectories of the three contacts

z

initial pointend point

Fig. 5. View 1: trajectories of the three fingers

Problem 1: Max-transfer ProblemFind ~αo such that

g1(~αo) = minwT

o (GGT )−1wo=1max

Gx=wo,P (x)≥0

wTo Awo

xT x(15)

is maximal.Since it is impossible to endow a bi-invariant metric on thespace of external wrenches, we usually assign a left invariantmetric

‖wo‖2wo= wT

o Awo , A > 0.

Note that the problem in the current form is slightly differentfrom that of Ferrari and Canny, and Mirtich and Canny in thatthe constraintswT

o (GGT )−1wo = 1 is position dependent.Example 1: Planning of optimal grasps usingg1 for a

three-fingered hand manipulating an ellipsoidConsider the case of a three-fingered hand manipulating anellipsoid through frictional point contacts. We parameterize

−1−0.5

00.5

1

−1

−0.5

0

0.5

1

−3

−2

−1

0

1

2

3

x


y

z



−1−0.5

00.5

1

−1

−0.5

0

0.5

1

−3

−2

−1

0

1

2

3

y

x


z



the ellipsoid by the longitude and latitude coordinates

αo =[

uo

vo

]→

acos uocos vo

bcos uosin vo

csinuo

with a = b = 1 and c = 3. Initially, the three fingers arearbitrarily placed at the three pointsαo1 = (0, 0)T , αo2 =(0, π

4 )T , andαo3 = (π8 ,−π

4 )T . We useg1 to plan trajectoriesof the three fingers so that1g1

is minimized. A is chosen tobe I. To apply Algorithm1 and2 of Section IV, we need tocalculate∇wo

xT xwT

o woand∇~αo

xT xwT

o wo. Note that

xT x = wTo (GGT )−1wo + yT V T V y,

we have

∂ xT xwT

o wo

∂wo= − 2xT x

(wTo wo)2

wo + 2(GGT )−1wo

wTo wo


and ∇wo

xT xwT

o wois its projection to the constraint subspace

wTo (GGT )−1wo = 1. ∇~αo

xT xwT

o wois calculated as

∇~αo

xT x

wTo wo

=∇~αo

xT x

wTo wo

=wT

o∂(GGT )−1

∂~αowo + yT ∂V T V

∂~αoy

wTo wo

.

The final simulation results are shown in Fig. 4, 5, 6, and 7.In this example, the computation time for an optimal solutionis about 4 hours in P4 and Win2000 (typically4-5 hours,depends on the initial conditions and used step sizes). Withoutspecifically pointed out, all our simulations are performed inthe same system.

Remark 2: One problem of using the left invariant met-ric is that different choices ofA will in general lead todifferent optimal grasps. Since force and moment are twodifferent quantities which can be measured both in a physicallymeaningful way, Mirtich and Canny considered to optimizeboth the force and moment transfer function. Some successfulapplications of this method to optimal grasp planning canbe found in [24] and [25]. For two-fingered planar grasps,the antipodal grasp with the largest distance between the twocontact points is found to be the optimal, and for three-fingeredplanar grasps the equilateral grasp with the maximal outertriangle (symmetric grasps if the object is a circle) is the best.

B. Max-normal-grasping-force Problem

For frictional point contacts, the normal component of thefinger forcexi is xi,3. We define

xn =k∑

i=1

xi,3 = ξT x > 0

as the normal grasping force, whereξ = [0, 0, 1, · · · ,0, 0, 1]T .Note that for a balance graspGx = wo, xn measureshow stable the grasp is. Physically, it represents how muchpassive forces it can produce to the object to resist externaldisturbances. Motivated by this, we introduce the followingproblem:

Problem 2: Max-normal-grasping-force ProblemFind grasp~αo such that

g2(~αo) = maxwT

o Awo−1=0min

Gx=wo,P (x)≥0

1ξT x

(16)

is minimal.This is amin-max-min problem. For given~αo and wo, theproblem in the most internal layer of (16)

maxGx=wo,P (x)≥0

ξT x

can be transformed into a semi-definite problem [37] [38].Example 2: Optimal grasp planning using g2: Example

1 continuedIn this example, we adoptg2 to optimize the grasp for theellipsoid of Example1 with the same initial grasp as before.First, we compute∇woξ

T x and∇~αoξT x as follows. Since

ξT x = ξT GT (GGT )−1wo + ξT V y,

Fig. 8. Trajectory of the cost function

−1

−0.5

0

0.5

1−1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1

−3

−2

−1

0

1

2

3Trajectories of the three contacts

yx

z



−1−0.500.51

−1 −0.5 0 0.5 1

−3

−2

−1

0

1

2

3

x


y

z




−1

0

1−1 −0.5 0 0.5 1

−3

−2

−1

0

1

2

3

x


y

zinitial pointend point


we have∂ξT x

∂wo= (GGT )−1Gξ

and∇woξT x is its projection towT

o Awo = 1. Similarly,

∇~αoξT x = ξT ∂GT (GGT )−1

∂~αowo + ξT ∂V

∂~αoy.

Using Algorithm1 and2 of Section IV, we obtain simulationresults as shown in Fig. 8, 9, 10, and 11. In this example, ittakes about2 hours to compute an optimal solution.

C. Min-analytic-center Problem

From the works on grasping force optimization [10], [11],[9], [28], we see that given~αo andwo, we can assign a uniqueanalytic center toΩx := x | Gx = wo, P (x) > 0 as

φ(~αo, wo) = minx logdetP (x)−1

subject toGx = wo

P (x) > 0.

The smallerφ is, the farther the optimalx is from the boundaryof the friction cone (i.e. more stable). Based on this, weformulate the following problem

Problem 3: Min-analytic-center ProblemFind grasp~αo, such that

g3(~αo) = maxwTo Awo=1 φ(~αo, wo)

= maxwTo Awo=1 minGx=wo,P (x)>0 logdetP (x)−1

is minimal.Clearly, the Min-analytic-center problem is amin-max-minproblem. Given ~αo and wo, φ(~αo, wo) can be solved asfollows. Eliminating the force balance equation by substitutingx = x0 + V y into φ yields

φ(~αo, wo) = miny

logdetP (y)−1 (17)

subject toP (y) = P (x0 + V y) = A0 + A1y1 + · · ·+ An−6yn−6 > 0

where A0 =∑

i Aix0,i, Ai =∑

j AjVj,i, x0 =[x0,1, · · · , x0,n]T , and Vj,i is the jith element ofV . If thesolution set forP (y) > 0 is empty, i.e., Problem (17) isinfeasible, the system is not force closure at~αo. For thesegrasp configurations, we assign a large number tog3, e.g.,2000. Otherwise, an optimal solution can be obtained. Denoteby SN

++ the set of positive definiteN × N matrices. SincelogdetP−1 →∞ as P (y) goes to the boundary ofSN

++, it isminimal if and only if its gradient is equal to zero, i.e.,

∂logdetP (y)−1

∂yj= −Tr(P (y)−1Aj) = 0 , j = 1, · · · , n− 6.

Let y∗ be the solution of the above equalities, which can beshown to be unique and smoothly depend on both~αo andwo

[31]. Although it is hard to derive the analytical expression ofy∗(~αo, wo), Problem (17) is a standard analytic-center problemand can be solved numerically by interior point algorithms [38]when~αo andwo are given in advance.

Example 3: Planning of optimal grasps usingg3: Exam-ple 1 continuedConsider again the grasp case in Example1 with the ini-tial configuration: αo1 = (−0.2684,−0.0820)T , αo2 =(−0.2097, 1.4009)T , and αo3 = (0.7099,−1.3849)T . Al-gorithm 1 and 2 of Section IV require us to compute∇~αo

logdetP (y)−1 and∇wo logdetP (y)−1. Note that

∂logdetP−1

∂P= −P−1.

We have

∂logdetP (y)−1

∂wo= −∂vec(A0)

∂wo

T

vec(P−1)= −(GGT )−1GHvec(P−1)

and∇wo logdetP (y)−1 is its projection towTo Awo = 1, where

H =

vec(A1)T

...vec(An)T

,

and vec denotes the vector operator.∇~αologdetP (y)−1 is

given by

−(wTo

∂(GGT )−1G

∂~αo+ yT ∂V T

∂~αo)Hvec(P−1).

The simulation results are shown in Fig. 12, 13, 14, 15, and16. From Fig. 12 and 13, we conclude that usingg3, andAlgorithm 1 and 2, the grasp will evolve from non-force-closure states to force-closure states asg3 goes from2000(non-force-closure states) to a much smaller value (force-closure states). Second, the non-convergence ofg3 in Fig. 13is because the grasps goes into states where we can applyarbitrary large normal finger forces to the object, anddetPwill go to ∞ (correspondingly,logdet(P−1) to −∞). In thisexample, it typically takes2 hours for800 iterations. To ensurethe convergence of the algorithm, we can add a linear termξT x to the quality function so as to restrict the normal graspingforce. Then the original quality function is changed into

g3(~αo) = maxwT

o Awo=1min

Gx=wo,P (x)>0ξT x + logdetP (x)−1.


Fig. 12. Trajectory of the cost function: from non-force-closure to force-closure

Fig. 13. Trajectory of the cost function: force closure part

−1 −0.5 0 0.5 1−1−0.5

00.5

1

−3

−2

−1

0

1

2

3

x


y

z


Fig. 14. View 1: trajectories of finger2 and3

−101

−1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1

−3

−2

−1

0

1

2

3

xy


z



−1−0.500.51−1 −0.5 0 0.5 1

−3

−2

−1

0

1

2

3

y


x

z


Fig. 16. View 3: trajectories of finger1 and2

Compared with the previous grasp synthesis problems, e.g.,those in [24] and [25], both the Max-normal-grasping-forceproblem and the min-analytic-center problem are formulatedbased on the optimal grasping forces. They are closely relatedto the real-time grasping force optimization problem [9], whichcan be formulated as a semi-definite programming problem, ananalytic center problem, or a max-det (determinant) problem.Thus, the physical meaning of the above two problems canbe explained as finding optimal grasp configurations such thatthe worst case optimal graphing forces is optimal. These twoproblems are general enough to be applied to objects withsmooth surfaces grasped by hands with any number of fingersas long as the geometric model of the surface is known.Powerful algorithms exist for computing an optimal graspingforce which will be used in Algorithm1 and 2, and therebyimprove their computation efficiency.

VI. T WO SIMPLIFIED PROBLEMS

The Max-transfer and Min-analytic-center problems canbe simplified via estimation. The derived simple analytic


expressions of their respective grasp quality functions aresuitable for real-time grasp planning.

A. Simplifying the Max-transfer Problem

FC is a subset with rotational symmetry (assume thatµt,i =µi for SFCE contacts). Its center of symmetry is a line passingthrough the vertexO (the origin ofRn, see Fig. 17) with thedirection given by

ξ = [ 1︸︷︷︸1

, · · · , 1︸︷︷︸k

]T for FPC

ξ = [0, 0, 1︸︷︷︸1

, · · · , 0, 0, 1︸︷︷︸k

]T for PCWF

ξ = [0, 0, 1, 0︸︷︷︸1

, · · · , 0, 0, 1, 0︸︷︷︸k

]T for SFCE.

The size ofFC is determined by the vector of cone anglesθc = (θ1, · · · , θk)T ∈ Rk with θi ∈ R, i = 1, · · · , k, beingthe angle of conei. θi = 0 for FPC contacts andθi = tan−1 µi

for PCWF and SFCE models.Without the friction cone constraints and adopting the idea

used in the Max-transfer problem, the maximal distance fromthe originO to the affine setAff = x | Gx = wo among allunit external wrenches measures the capability of finger forcesin resisting the external wrenches:

φ(~αo) = maxwT

o (GGT )−1wo=1d(O, Aff).

Here, we have implicitly used the2-norm for finger forcesand the metricA = (GGT )−1 for external wrenches. By theprojection theorem,d(O, Aff) is calculated as

d(O, Aff) = ‖GT (GGT )−1wo‖2i.e.,d is the magnitude of the manipulating force. To take intoaccount the friction cone constraints, we need to modify thedistanced into its projection to the center of the friction cone

dT (O, Aff ∩ FC) =|ξT GT (GGT )−1wo|

(ξT ξ)12

.

andφ(~αo) to

φT (~αo) = maxwT

o (GGT )−1wo=1dT (O, Aff ∩ FC).

In fact, at a given grasp configuration~αo, dT is maximal ifand only if (GGT )−1wo ‖ (GGT )−1Gξ, i.e.,

wo = ± 1γ Gξ

γ =√

ξT GT (GGT )−1Gξ

from which, we have

φT (~αo) =

√ξT GT (GGT )−1Gξ

(ξT ξ)12

.

We introduce the following Simplified Max-transfer problem:Problem 4: Simplified Max-transfer Problem

Find grasp configurations~αo such that

φT (~αo)

is minimal.Sinceξ is constant, the above problem is equivalent to

min~αo

ξT GT (GGT )−1Gξ.

Aff

t

O

x =G0+fo

d

ξ

Fig. 17. Relative configuration between Aff and the product of SOC

B. Simplifying the Min-analytic-center problem

The analytic centerx∗ for minx logdetP (x)−1 at a givengrasp configuration~αo and under a given external forcewo

(satisfyingwTo Awo = 1) can be physically interpreted as the

one which is farthest from the boundary of the friction cone.x∗

can be estimated as the intersection point between the centerof symmetryξ and the affine setAff, see Appendix A for acomplete derivation. Letx∗ = tξ, t > 0 ∈ R, then

Gξt = wo (18)

t2 =1

ξT GT AGξ(19)

x∗ =√

1ξT GT AGξ

ξ. (20)

From the Helmke, Hueper and Moore’s expression ofP (x),detP (x)−1 is calculated as

detP (x∗)−1 = 1Πk

i=1(µ2i x2

i,3−x2i,1−x2

i,2)|x∗

= (ξT GT AGξ)k

Πki=1µ2

i

.

Then,

g3(~αo) = log(ξT GT AGξ)k

Πki=1µ

2i

.

Since thelog function is monotone increasing, we introducethe following simplified Min-analytic-center problem:

Problem 5: Simplified Min-analytic-center problemFind grasp configurations~αo such that

g3 = ξT GT AGξ

is minimal.

C. Practical Examples

The objective functions of the simplified Max-transfer andthe simplified Min-analytic-center problems are simple an-alytic functions. They can be used to efficiently determinethe optimal grasps of several classical examples. It turnsout that the obtained optimal grasps coincide with those bythe traditional heuristic approaches. Moreover, the developedquality measures are general and suitable for real-time graspoptimization.


Example 4: Antipodal Configurations: Optimal 2-fingered GraspsConsider a2-fingered hand grasping a spherical object with

radiusR, as shown in Fig. 2. The hand makes contacts with theobject atαo1 = (uo1, vo1)T and αo2 = (uo2, vo2)T . Assumethat both contacts are SFCE, the grasp map is calculated as

G =[

G1 G2

]

Gi =

−suoicvoi

svoicuoi

cvoi0

−suoisvoi −cvoi cuoisvoi 0cuoi

0 suoi0

RsvoiRsuoi

cvoi0 cuoi

cvoi

−RcvoiRsuoi

svoi0 cuoi

svoi

0 −Rcuoi 0 suoi

i = 1, 2

where cuoi = cos uoi, cvoi = cos voi, suoi = sin uoi, andsvoi

= sin voi. The finger forces are restricted to the frictioncones

FC = FC1 ×FC2

with the center of symmetryξ = [0, 0, 1, 0, 0, 0, 1, 0]T . Apply-ing the simplified Min-analytic-center problem by substitutingG andξ into g3 , we obtain

g3 = ξT GT AGξ = ‖

cuo1cvo1 + cuo2cvo2

cuo1svo1 + cuo2svo2

suo1 + suo2

000

‖2,

whereA is assumed to be the identity matrix (the same resultswill be obtained ifA is chosen to be other positive definitematrices). It is minimal if and only if

[cuo1cvo1 , cuo1svo1 , suo1 ]T = −[cuo2cvo2 , cuo2svo2 , suo2 ]

T .

That is, the two contacts are antipodal. It should be notedthat antipodal grasps are also the optimal solutions for thesimplified Max-transfer problem.

In general, if the grasped object has a complex geometry,

g3 = ξGT AGξ = ‖[

n(αo1) + n2(αo2)X(αo1)× n(αo1) + X(αo2)× n(αo2)

]‖2,

which is equal to zero (minimal) if and only if

n(αo1) = −n(αo2)(X(αo1)−X(αo2))× n(αoi) = 0 , i = 1, 2.

Again, the optimal solutions are antipodal grasps. Antipodalgrasps are not unique for a given object. To determine theoptimal one from the set of antipodal grasps, we need to goback to the Max-transfer problem and find the grasp with themaximal distance to be the optimal one [25].

Simply applying the gradient algorithm,g3 can also be usedto plan trajectories of the two contact points from an arbitraryinitial configuration to the optimal one. In the current case,we have

g3 = 2(1 + suo1suo2 + cuo1cuo2cvo1−vo2)

0 5 10 15 20 25 30 35 40 45 500

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

No. of iterations

Val

ue o

f cos

t fun

ctio

n

Approaching antipodal configuration through gradient algorithm

(a)

0 5 10 15 20 25 30 35 40 45 50188

190

192

194

196

198

200

No. of iterations

Dis

tanc

e be

twee

n tw

o co

ntac

t poi

nts

Approaching antipodal configuration through gradient algorithm

(b)

Fig. 18. (a)Trajectory ofg3 (b) Distance between two fingers

−100−50

050

100

−40

−20

0

20

40

60

−1

−0.5

0

0.5

1

x−coordinate

y−coordinate

z−co

ordi

nate

Trajectory of two contacts on a sphere

Contact 1Start pointEnd pointContact 2

Fig. 19. Trajectories of the two contact points

where cvo1−vo2 = cos(vo1 − vo2). Its Euclidean gradient iscalculated as

∇~αog3 =

2(cuo1suo2 − suo1cuo2cvo1−vo2)−2cuo1cuo2svo1−vo2

2(suo1cuo2 − cuo1suo2cvo1−vo2)2cuo1cuo2svo1−vo2

.

Fig. 18-(a) and 18-(b) give trajectories ofg3 and the distancebetween the two fingers of the hand, respectively. It clearlyshows that the grasp tends to be antipodal (see Fig. 19).The parameters used areR = 100mm, αo1(0) = (0, 0) andαo2(0) = (0, 2.5). The computation time for achieving thefinal antipodal configuration is0.047 second, which showsthat the computation time has been greatly reduced comparedwith those in Section V.

Example 5: Symmetric Configurations: Optimal 3-fingered Grasps of a Spherical ObjectConsider a3-fingered hand grasping a spherical object of

radiusR. PCWF model is assumed for the three contacts withlocal coordinatesαo1 = (uo1, vo1)T , αo2 = (uo2, vo2)T , andαo3 = (uo3, vo3)T . The grasp map is given by

G =[

G1 G2 G3

],


0 5 10 15 20 25 30 35 40 45 500

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

No. of iterations

Val

ue o

f cos

t fun

ctio

n

Convergence of the cost function to zero

(a)

0 5 10 15 20 25 30 35 40 45 50460

470

480

490

500

510

520

No. of iterations

mm

Sum of lengths of the three sides of the contact triangle

(b)

Fig. 20. (a)Trajectory ofg3 (b) The sum of the length of three sides of thegrasp triangle

where

Gi =

−suoicvoi

svoicuoi

cvoi

−suoisvoi −cvoi cuoisvoi

cuoi 0 suoi

RsvoiRsuoi

cvoi0

−Rcvoi Rsuoisvoi 00 −Rcuoi

0

, i = 1, · · · , 3.

The finger forces are restricted to

FC = FC1 ×FC2 ×FC3

with the center

ξ = [0, 0, 1, 0, 0, 1, 0, 0, 1]T .

Applying the simplified Min-analytic-center problem, the costfunction is calculated as

g3 = ‖

cuo1cvo1 + cuo2cvo2 + cuo3cvo3

cuo1svo1 + cuo2svo2 + cuo3svo3

suo1 + suo2 + suo3

000

‖2.

It is minimal if and only if

cuo1cvo1 + cuo2cvo2 + cuo3cvo3

cuo1svo1 + cuo2svo2 + cuo3svo3

suo1 + suo2 + suo3

= 0.

This clearly shows that the three fingers should be at threesymmetric points of a big circle of the object. The sameconclusion can also be reached by applying the simplifiedMax-transfer problem.

In general, when the object is not spherical, the cost functiong3 is given by

‖[

n(αo1) + n2(αo2) + n3(αo3)X(αo1)× n(αo1) + X(αo2)× n(αo2) + X(αo3)× n(αo3)

]‖2

and it is zero (or minimal) if and only if∑3

i=1 n(αoi) = 0∑3i=1 X(αoi)× n(αoi) = 0.

The first equality means that the three normal vectors are120 from each other. The two equalities together mean that

−100

−50

0

50

100

−100

−50

0

50

100−1

−0.5

0

0.5

1

mm mm

mm

Trajectory of three−fingered grasps

Contact locationsStart pointEnd point

Fig. 21. Trajectory of the grasp configuration

the three normal vectors intersect at the same point, and arecontained in one plane. This is exactly what Mirtich and Canny[25] termed a symmetric grasp. Again, all such grasps form anonempty set for a given object. To find the optimal one fromthis set, we go back to the Max-transfer problem and find thegrasp with the largest outer triangle.

Second, we useg3 and its respective gradient algorithm tooptimize the grasp from an arbitrary initial configuration. Notethat

g3 = 3 + 2(suo1suo2 + suo2suo3 + suo1suo3

+ cuo1cuo2cvo1−vo2 + cuo2cuo3cvo2−vo3

+ cuo1cuo3cvo1−vo3).

Its Euclidean gradient∇g3 is given by

2[cuo1(suo2 + suo3)− suo1(cuo2cvo1−vo2 + cuo3cvo1−vo3)]−2cuo1(cuo2svo1−vo2 + cuo3svo1−vo3)



.

The trajectories ofg3 and the sum of the length of thethree sides of the triangle formed by the three contact pointsare shown in Fig. 20-(a) and 20-(b), respectively. Fig. 21gives the trajectories of the three contact points of the hand.Here, we have used the following parametersR = 100mm,αo1(0) = (0, 0), αo2(0) = (0, 2.5) and αo3(0) = (0, 3.3).As is expected, the grasp tends to be symmetric as the sumof the length of the three sides of the triangle formed by thethree contact points approaches to3

√3R. It takes0.047s to

arrive at the symmetric grasp configuration, showing again theefficiency of the simplified problem.

VII. A PPLICATION TO COORDINATED MANIPULATION

WITH CONTACT POINTS SERVOING

In this section, we apply real-time grasp planning to coordi-nated manipulation with contact points servoing. As shown inFig. 1, given a desired trajectory of the grasped object,gd

po(t),we wish to find the corresponding finger velocityVpfi that:(1) executes the desired object trajectory, and (2) maintains oroptimizes the grasp quality. Readers are referred to [20], [9] for


Fig. 22. The HKUST3-fingered hand manipulating a spherical object

0 20 40 60 80 100 120 140 160 180 2001.605

1.61

1.615

1.62

1.625

1.63

1.635

1.64

1.645

1.65

50ms

rad

Desired trajectory of vo1

of finger 1

(a)

20 40 60 80 100 120 140 160 180 2000

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

50ms

mm

Trajectory tracking of vf1 of finger 1

Actual Trajectory vf1(t)

Desired Trajecotry vf1d

(t)

(b)

Fig. 23. (a) Finger 1: Desired trajectory ofvo1, (b) Finger 1: Trajectorytracking ofvf1.

generation of optimal finger forces that balance a given objectwrench. The transformation takingO to P can be expressedas

gpo = gpfi

gfilfi

glfi

loig

loio

from which we have

Adgloi

oVpo = Adg−1

filoi

Vpfi −Adgloi

lfi

Vloilfi

and thus

Vpfi = Adgfio

Vpo + Adgfilfi

Vloilfi

. (21)

In (21), the object velocityVpo is the input and the fingertipvelocity Vpfi is the output to be determined. We wish tospecify the contact velocityVloi

lfi∈ R6 so that (i) grasp

quality (and thus force closure condition) is maintained oroptimized, and (ii) the fingers impart on the object a desiredobject wrench. In order to prevent sliding, and maintain fingerforces inside the friction cone, we impose the following

0 20 40 60 80 100 120 140 160 180 200−1.65

−1.645

−1.64

−1.635

−1.63

−1.625

−1.62

−1.615

−1.61

−1.605

50ms

rad

Desired trajectory of vo2

of finger 2

(a)

0 20 40 60 80 100 120 140 160 180 200−4.5

−4

−3.5

−3

−2.5

−2

−1.5

−1

−0.5

0

50ms

mm

Trajectory tracking of vf2 of finger 2

Actual Trajectory vf2(t)

Desired Trajecotry vf2d

(t)

(b)


constraints on the contact velocity:

Vloilfi

=

0 00 00 01 00 10 0

[ωi

x

ωiy

]:= Bc

ci

[ωi

x

ωiy

],

where (ωix, ωi

y)T are relative rolling velocities to be deter-mined. By inverting Montana’s kinematic equations of contact,

[ωi

x

ωiy

]= Rψi(Koi + Kfi)Moiαoi

where (Koi + Kfi) is the relative curvature form,Moi themetric form of the object [29], and

Rψi =[

cosψi −sin ψi

−sinψi −cos ψi

].

We specifyαoi using the negative gradient of the grasp qualityfunction

g3 : ~αo → R

and~αo = −λ∇g3(~αo) , λ ∈ (0, 1).

Let xdi be the optimal finger forces needed to balance an object

wrench, (see [10], [9] for computation ofxdi ). The net finger

velocity required to accomplish all the objectives is given by

Vpfi = Adgfio

(ηi)V dpo

+ Adgfilfi

Bcci

Rψi(Koi + Kfi)Moi(−λ∇ig3(~αo))

+ Ci(xdi − xi) (22)

whereCi ∈ R6×ni is a compliance matrix, andxi the actualfinger force. Note that in (22), the first term allows the fingerto accommodate the object motionV d

po, the second term servosthe contact points to an optimal grasp configuration and thelast term enables the fingers to impart a net object wrenchwith optimal finger forces. Once the hand achieves an optimalgrasp configuration, the second correction term disappearswith vanishing of the gradient vector field.

Remark 3: During manipulation, the real-time optimiza-tion of the grasp quality function is necessary to keep the


0 20 40 60 80 100 120 1402.636

2.638

2.64

2.642

2.644

2.646

2.648

2.65

50ms

rad

Finger 1: Trajectory of vo1d

(t) (uo1d

(t)=0)

(a)

0 20 40 60 80 100 120 140−1.6

−1.4

−1.2

−1

−0.8

−0.6

−0.4

−0.2

0

50ms

mm

Finger 1: Trajectory tracking of vf1

(uf1

=0)

Desired trajectory vf1d

(t)Actual trajectory vf

1(t)

(b)


stability of the system, and thereby prevent the dropping ofthe object [21]. Here, we adopt the simplified grasp qualityfunction g3, which makes the real-time execution of manipu-lation tasks possible as suggested in the simulation example(0.047 s).

Remark 4: The controller (22) is only suitable for handswith fingers of six degree-of-freedoms. If the desired fingermotion is not realizable, e.g., fingers with less than six degree-of-freedoms, then we have to take the finger kinematics intoaccount as a kind of constraints. In future works, we wish toextend the controller (22) to such systems.

Several experiments are conducted with the HKUST3-fingered hand (see Fig. 22). Each finger of the hand consists ofa Motorman K-3S robot equipped with force/torque sensor anda 16×16 tactile array fingertip. A VME based multiprocessorcontrol system with three8-axis DSP motion control boardsis provided for joint-level control and two Motorola 68040processors are used for object-level motion and grasping forcecontrol, along with a VxWorks real-time operating system anda Sun workstation. In the experiments, the object is required tomove100 mm along the twist(0, 0, 1, 0, 0, 0) in 10 seconds.

We first manipulate a ball of radiusR = 93mm using onlytwo fingers, the initial coordinates of the two contacts areαo1(0) = (0, ( 1

2 + 140 )π)T and αo2(0) = (0, (− 1

2 − 140 )π)T .

The desired curves of the two contacts, as planned in Example5, are given in Fig. 23-(a) and 24-(a), which show that the twofingers tend to be antipodal at the big circleuo = 0. Fig. 23-(b)and 24-(b) show the trajectory tracking results ofvfi (ufi =0), i = 1, 2. The desired curvesvd

fi(t) are obtained fromvd

oi(t) by inverting Montana’s contact kinematics equations[30], [29].

In the second experiment, we manipulate a ball of radiusR = 122mm using three fingers. Other parameters areαo1(0) = (0, ( 5

6 + 1100 )π)T , αo2(0) = (0, (− 1

2 − 140 )π)T , and

αo3(0) = (0, ( 16 + 1

40 )π)T . The desired curves of the threecontacts, as planned in Example6, are shown in Fig. 25-(a),26-(a), and 26-(a), from which we can see that the three fingerstend to the three symmetric points of the great circleuo = 0.The trajectory tracking results ofvfi (ufi = 0), i = 1, · · · , 3are shown in Fig. 25-(b), 26-(b) and 27-(b), respectively.

0 20 40 60 80 100 120 140−1.65

−1.64

−1.63

−1.62

−1.61

−1.6

−1.59

−1.58

50ms

rad


(t) (uo2d

(t)=0)

(a)

20 40 60 80 100 120 1400

1

2

3

4

5

6

7

8

9

50ms

mm


(uf2

=0)



2(t)

(b)


0 20 40 60 80 100 120 1400.55

0.56

0.57

0.58

0.59

0.6

0.61

50ms

rad


(t) (uo3d

(t)=0)

(a)

0 20 40 60 80 100 120 140−6

−5

−4

−3

−2

−1

0

50ms

mm


(uf3

=0)



3(t)

(b)


VIII. C ONCLUSION

This paper presented a general formulation of the optimalgrasp synthesis problem as optimization problem of threegrasp quality functions. We discussed physical significancesand gave algorithms for computing the gradient solutions ofthese quality functions. We also provided simplified versionsof two problems when real-time grasp planning solutions areneeded. we showed in particular that optimal solutions ofthe simplified problems coincide with the familiar optimalgrasps obtained using heuristic approaches. We applied real-time grasping optimization to coordinated manipulation withcontact points servoing. Simulation and experimental studieswere conducted to illustrate validity of the proposed methods.

In future works, we wish to extend the methods andalgorithms to objects with edges and vertices and obtainaccelerated results when objects have special geometries, e.g.,polyhedral objects.

APPENDIX A: A N APPROXIMATE SOLUTION TO THE

ANALYTIC -CENTER PROBLEM

The analytic-center problem

minGx=wo,P (x)>0

logdetP (x)−1

is equivalent to

maxGx=wo,P (x)>0

logdetP (x). (23)


Based on the structure of the matrixP (x) (5), we have

detP (x) = Πki=1(µ

2i x

2i,3 − x2

i,1 − x2i,2) ≤ Πk

i=1µ2i x

2i,3.

Here, without loss of generality we assume that the frictionmodel is PCWF. Note also that all finger forcesx with xi,1 =xi,2 = 0 andxi,3 > 0, i = 1, · · · , k, will automatically satisfyP (x) > 0. Thus a sufficient condition forx? to be a solutionof (23) is x? = tξ, t > 0 andGx? = wo with

ξ = [0, 0, 1︸︷︷︸1

, · · · , 0, 0, 1︸︷︷︸k

]T .

If x = tξ and Gx = wo can not be simultaneously satisfied,we seek at as in (18), (19), and (20), such that‖Gξt‖ = ‖wo‖is satisfied, which, of course, only provides an approximationof the solution to (23). We could also adopt the followingoptimal approximation

mint‖Gξt− wo‖.

However, the resultant optimal solution is still a function ofwo. This will not simplify too much of the cost function.

REFERENCES

[1] F. Reuleaux. Theoretische Kinematik. Translated as Kinematics ofMachinery, Dover, New York, 1875.

[2] J.K. Salisbury and B. Roth. Kinematic and force analysis of articulatedmechanical hands. ASME J. Mech. Transmissions Automat. Des.,105(1):35–41, 1983.

[3] B. Mishra, J.T. Schwartz, and M. Sharir. On the existence and synthesisof multifinger positive grips.Algorithmica, 2(4):541–558, 1987.

[4] I.M. Chen and J.W. Burdick. Finding antipodal point grasps onirregularly shaped objects.IEEE Trans. Robot. Automat., 9(4):507–512,Aug. 1993.

[5] V. Nguyen. Constructing force-closure grasps.Int. J. Robot. Res.,7(3):2–17, 1988.

[6] J.C. Trinkle. On the stability and instantaneous velocity of graspedfrictionless objects.IEEE Trans. Robot. Automat., 8(5):560–572, Oct.1992.

[7] A. Bicchi. On the closure properties of robotic grasping.Int. J. Robot.Res., 14(4):319–334, 1995.

[8] Y.H. Liu. Qualitative test and force optimization of 3-D frictional form-closure grasps using linear programming.IEEE Trans. Robot. Automat.,15(1):163–173, Feb. 1999.

[9] L. Han, J.C. Trinkle, and Z.X. Li. Grasp analysis as linear matrixinequality problems.IEEE Trans. Robot. Automat., 16(6):663–674, Dec.2000.

[10] M. Buss, H. Hashimoto, and J. Moore. Dextrous hand grasping forceoptimization. IEEE Trans. Robot. Automat., 12(3):406–418, Jun. 1996.

[11] M. Buss, L. Faybusovich, and J. Moore. Dikin-type algorithms fordextrous grasping force optimization.Int. J. Robot. Res., 17(8):831–839, 1998.

[12] Z. Ji and B. Roth. Contact force in grasping and kinematic constraints.In Seventh IFTOMM World Congress, pages 1219–1222, 1987.

[13] J. Ponce and B. Faverjon. On computing three-finger force-closuregrasps of polygonal objects.IEEE Trans. Robot. Automat., 11(6):868–881, Dec. 1995.

[14] J. Ponce et al. On computing four-finger equilibrium and force-closuregrasps of polyhedral objects.Int. J. Robot. Res., 16(1):11–35, 1997.

[15] D. Ding, Y.H. Liu, M.Y. Wang, and S.G. Wang. Automatic selection offixturing surfaces and fixturing points of polyhedral workpieces.IEEETrans. Robot. Automat., 17(6):833–841, Dec. 2001.

[16] Y.H. Liu. Computing n-finger form-closure grasps on polygonal objects.Int. J. Robot. Res., 18(2):149–158, 2000.

[17] M. Cutkosky.Robotic Grasping and Fine Manipulation. Boston: KluwerAcademic Publishers, 1985.

[18] Z. Li and S. Sastry. Task oriented optimal grasping by multifingeredrobotic hands. InProceedings of IEEE International Conference onRobotics and Automation, pages 389–394, 1987.

[19] J.W. Hong, G. Lafferriere, B. Mishra, and X.L. Tang. Fine manipu-lation with multifinger hands. InProceedings of IEEE InternationalConference on Robotics and Automation, pages 1568–1573, 1990.

[20] Z.X. Li, Z. Qin, S. Jiang, and L. Han. Coordinated motion generationand real-time grasping force control for multifingered manipulation.In Proceedings of IEEE International Conference on Robotics andAutomation, pages 3631–3638, 1998.

[21] S.L. Jiang, K.K. Choi, and Z.X. Li. Coordinated motion generationfor multifingered manipulation using tactile feedback. InProceedingsof IEEE International Conference on Robotics and Automation, pages3032–3037, 1999.

[22] L. Han et al. Dextrous manipulation with rolling contacts. InProceed-ings of IEEE International Conference on Robotics and Automation,pages 992–997, 1997.

[23] D. Kirkpatrick, B. Mishra, and C.K. Yap. Quantitative steinitz’s theoremwith applications to multifingered grasping.Discrete and ComputationalGeometry, 7(3):295–318, 1992.

[24] C. Ferrari and J. Canny. Planning optimal grasps. InProceedingsof IEEE International Conference on Robotics and Automation, pages2290–2295, 1992.

[25] B. Mirtich and J. Canny. Easily computable optimum grasps in 2-D and3-D. In Proceedings of IEEE International Conference on Robotics andAutomation, pages 739–747, 1994.

[26] X.Y. Zhu, H. Ding, and J. Wang. Grasp analysis and synthesis based on anew quantitative measure.IEEE Trans. Robot. Automat., 19(6):942–953,Dec. 2003.

[27] X.Y. Zhu and J. Wang. Synthesis of force-closure grasps on 3-D objectsbased on the Q-distance.IEEE Trans. Robot. Automat., 19(4):669–679,Aug. 2003.

[28] G.F. Liu and Z.X. Li. Real-time grasping force optimization for mul-tifingered manipulation: Theory and experiments.IEEE/ASME Trans.Mechatron., 9(1), Mar. 2004.

[29] R. Murray, Z.X. Li, and S. Sastry.A Mathematical Introduction toRobotic Manipulation. CRC Press, 1994.

[30] D.J. Montana. The kinematics of contact and grasp.Int. J. Robot. Res.,7(3):17–25, 1988.

[31] U. Helmke, K. Hueper, and J.B. Moore. Quadratically convergentalgorithms for optimal dextrous hand grasping.IEEE Trans. Robot.Automat., 18(2):138–146, Apr. 2002.

[32] M.D. Do Carmo. Differential Geometry of Curves and Surfaces.Prentice-Hall, Inc., Englewood Cliffs, New Jersey, 1976.

[33] T. Omata. Fingertip position planning for a multifingered hand. InIEEE International Workshop on Intelligent Robots and Systems, pages859–864, 1990.

[34] B. Faverjon and J. Ponce. On computing two-finger force-closure graspsof curved 2D objects. InProceedings of IEEE International Conferenceon Robotics and Automation, pages 424–429, 1991.

[35] V.F. Demyanov and V.N. Malozernov.Introduction to Minmax. DoverPublications, 1990.

[36] T. Yoshikawa and K. Nagai. Manipulating and grasping forces in ma-nipulation by multifingered robot hands.IEEE Trans. Robot. Automat.,7(1):67–77, Feb. 1991.

[37] S. Boyd and L. Vandenberghe.Convex Optimization. CambridgeUniversity Press, 2004.

[38] L. Vandenberghe, S. Boyd, and S.-P. Wu. Determinant maximizationwith linear matrix inequality constraints.SIAM Journal on MatrixAnalysis and Applications, 19(2):499–533, 1998.

PLACEPHOTOHERE

Guanfeng Liu received the B.E.degree in ElectricalEngineering from Zhejiang University, Hangzhou,China, in 1998, and the Ph.D. degree in Electricaland Electronic Engineering from the Hong KongUniversity of Science and Technology in 2003. Hisresearch interests include multifingered manipula-tion, parallel manipulators, and nonlinear systemtheory.


PLACEPHOTOHERE

Jijie XU received the B.E. degree in Control Scienceand Engineering from Harbin Institute of Technol-ogy, China, in 2001. He is now a Ph.D. candidatein electrical and electronic engineering, Hong KongUniversity of Science and Technology. His researchinterests include multifingered manipulation, grasp-ing, and dextrous manipulation.

PLACEPHOTOHERE

Xin Wang received the B.E. degree in Electricand Electronical Engineering from Jinlin Institute ofTechnology, China, in 1994. She received the M.S.and Ph.D. degree in Automobile Engineering fromJilin University, Changchun, China, in 1997 and in2000. From 2001 to 2003, she worked as a ResearchAssociate in Electrical and Electronic Engineering inHong Kong University of Technology, Hong Kong.She is currently an Associate Professor in Con-trol Science and Control Engineering in ShenzhenGraduate School of Harbin Institute of Technology,

Shenzhen, China. Her research interests include motion control, parallelmanipulation and visual servoing.

PLACEPHOTOHERE

Zexiang Li received the B.S. degree in electrical en-gineering and economics (with honor) from CarnegieMellon University, Pittsburgh, PA, in 1983, and theM.A. degree in mathematics and Ph.D. degree inelectrical engineering and computer science, bothfrom the University of California at Berkeley, in1985 and 1989, respectively. He is an AssociateProfessor at the EEE Department, Hong Kong Uni-versity of Science and Technology. His researchinterests include robotics, nonlinear system theory,and manufacturing.

New IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. …ai.stanford.edu/~liugf/grasp-opt.pdf ·...

Documents

Transcript of New IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. …ai.stanford.edu/~liugf/grasp-opt.pdf ·...