IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007 1117 Fast Computation of Optimal Contact Forces Stephen P. Boyd,Fellow,IEEE, and Ben Wegbreit, Member, IEEE

Abstract—We consider the problem of computing the smallest clude [2] and [7], where the FOP is formulated (approximately) contact forces, with point-contact friction model, that can hold an as a linear program (LP), by approximating the friction force object in equilibrium against a known external applied force and limit constraints as linear inequalities; other papers taking the torque. It is known that the force optimization problem (FOP) can be formulated as a semidefinite programming problem (SDP) same approach include [8] and [9]. Mishra [10] describes several or a second-order cone problem (SOCP), and thus, can be solved types of FOPs, algorithms for determining feasible or optimal using several standard algorithms for these problem classes. In this forces, and points out the underlying convexity of the problem. paper, we describe a custom interior-point algorithm for solving the Other researchers have proposed neural network methods [11], FOP that exploits the specific structure of the problem, and is much [12], probabilistic algorithms [13], and various other methods. faster than these standard methods. Our method has a complexity that is linear in the number of contact forces, whereas methods The FOP comes up in several applications and settings, such based on generic SDP or SOCP algorithms have complexity that is as grasp optimization (which might involve the selection of cubic in the number of forces. Our method is also much faster for the contact points as well as forces) [14]–[18], or real-time smaller problems. We derive a compact dual problem for the FOP, grasp control [19], [20], and force optimization for the legs of which allows us to rapidly compute lower bounds on the minimum aquadrupedrobot[21].Someexperimentalresultsforforce contact force and certify the infeasibility of a FOP. We use this dual problem to terminate our optimization method with a guaranteed optimization are reported in, e.g., [22]. accuracy. Finally, we consider the problem of solving a family of In the late 1990s, several exact formulations of the FOP were FOPs that are related. This occurs, for example, in determining obtained by expressing it as a convex optimization problem in- whether force closure occurs, in analyzing the worst case contact volving matrix inequalities or second-order cone inequalities. force required over a set of external forces and torques, and in the The FOP is expressed in [24]–[27] using (linear) matrix in- problem of choosing contact points on an object so as to minimize the required contact force. Using dual bounds, and a warm-start equalities, so the resulting optimization problem is a semidefi- version of our FOP method, we show how such families of FOPs nite programming problem (SDP). The FOP is formulated as a can be solved very efficiently. second-order cone problem (SOCP) in [28]. Index Terms—Convex optimization, force closure, friction cone, These formulations reduce the problem to (what is now) a grasp force, interior-point method, second-order cone program standard convex optimization problem. This means the prob- (SOCP). lems can be solved, globally and efficiently, by a variety of methods for nonlinear convex optimization developed in the I. INTRODUCTION 1990s (see, e.g., [29]). General-purpose LP, SOCP, and SDP FUNDAMENTAL problem in robotics is choosing a set of software is now widely available (see, e.g., [30] for compar- A grasping (contact) forces for an object (see, e.g., the survey ative benchmarking of some recent codes). These solvers can [1]). The most basic requirement is the ability to restrain an reliably and efficiently solve FOPs. A typical FOP, with five object against a specified external wrench [2], such as that due to contact points and one external wrench, can be solved in well gravity. A generalization is the ability to resist external wrenches under a second, on the order of 100 ms on a current typical in a “task wrench space” [3] or any wrench due to a force applied desktop PC (for example, a 3-GHz Pentium IV). Several au- at the boundary of the object [4]. The ability to resist an arbitrary thors have developed custom solvers for the FOP that are faster external wrench is called force closure [5], [6]. In this paper, we than generic SDP or SOCP solvers. Buss et al. [25] developed first focus on the basic requirement, i.e., the ability of the contact aDikin-typealgorithmfortheFOP,andHelmkeet al. [27] de- forces to resist a specified external wrench; we then show how veloped a quadratically convergent algorithm. Our research is to efficiently handle some of these generalized contact force similar in spirit to these. requirements, such as determining force closure. The need to solve the FOP quickly arises in several applica- Among the contact forces that can hold the object in equilib- tions, for example, when the FOP must be solved many times. rium against the external wrench, we seek one with minimum Suppose we are given a lower and an upper bound on the exter- force, as measured by the maximum magnitude of the con- nal force and torque components, i.e., a box in wrench space, tact forces. The problem of finding such a set of forces is the and wish to find the maximum value of minimum contact force force optimization problem (FOP). Early papers on this topic in- required to resist any wrench in this wrench box. (This includes the more basic problem of determining whether or not each Manuscript received May 25, 2007. This paper was recommended for pub- wrench in the box can be resisted by some contact forces; by lication by Associate Editor F. Lamiraux and Editor F. Park upon evaluation of finding the maximum value of minimum force required, we ob- the reviewers’ comments. S. P.Boyd is with the Information Systems Laboratory, Department of Electri- tain a quantitative measure of the ability to resist wrenches in cal Engineering, Stanford University, Stanford, CA 94305-9510, USA (e-mail: the box.) We can do this by solving the FOP for each of the [email protected]). 26 =64vertices of the wrench box. The maximum of the opti- B. Wegbreit is with Strider Labs, Palo Alto, CA 94303, USA (e-mail: [email protected]). mal forces over these vertices is, in fact, the maximum contact Digital Object Identifier 10.1109/TRO.2007.910774 force required over all wrenches in the box, since the optimal

1552-3098/$25.00 © 2007 IEEE

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. 1118 IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007 force required is a convex function of the wrench. So, here, we we note that our method requires only around a factor of have an example where 64 FOPs must be solved. A similar ex- 10 times more effort than simply verifying that equilibrium ample is provided by the problem of determining force closure. holds. As we will see in Section II-F, this can be done by solving a set The outline of this paper is as follows. In Section II, we de- of seven (or more) FOPs. scribe the basic contact FOP, formulated as a conic problem, i.e., As another example where many FOPs must be solved, con- aconvexoptimizationproblemwithlinearobjectiveandequal- sider the problem of optimizing the position and orientation of ity constraints, and convex cone constraints on the variables. amanipulator,relativetoanobject,usingtheminimumforce In Section III, we derive a compact dual problem for the FOP, required to grasp the object as the objective (see, e.g., [6]). This which allows us to rapidly compute lower bounds on the min- is the (nonconvex) problem of optimizing the contact points at imum grasping force and certify the infeasibility of a FOP. We which to grasp a given object. This can be done using an outer use this dual problem to terminate our optimization method with search loop that generates candidate manipulator positions and aguaranteedaccuracy.InSectionIV,wedescribethebarrier orientations; for each candidate, we find the resulting contact subproblem associated with a primal interior-point method for points, and then, solve the associated FOP to determine the min- the FOP, and in Section V, we show how the special structure of imum force required to grasp the object. Such an algorithm can the FOP can be exploited to compute the search direction very require the solution of hundreds of FOPs. efficiently. We describe the overall algorithm in Section VI, and Applications that involve the solution of many FOPs, such methods for efficiently solving a family of FOPs in Section VII. as finding the worst case contact force over a wrench box or We give numerical results in Section VIII, and describe some contact point optimization, benefit directly from a very fast variations and extensions on the problem in Section IX. FOP solver like the one we describe in this paper. We will Finally, we describe our (fairly standard) notation. We de- also consider methods for obtaining even more efficiency when note the set of real numbers by R,thesetofrealn-vectors as n m n solving a family of related FOPs, using warm-start optimization R ,andthesetofrealm n matrices as R × .Weusethe techniques, and “short-circuiting,” i.e., early termination. notation (a, b, c) (for example)× to denote a column vector with In this paper, we describe a method for solving the FOP that components, and c,whichwealsowriteas is substantially faster than a general-purpose solver for SDP a or SOCP. Despite the speed of our method, its termination is b . entirely nonheuristic; our method terminates with a provable  c  bound on the suboptimality of the computed contact forces   (when the problem is feasible), or with a certificate proving When a, b,andc are reals, this is a vector in R3 .Weusethe infeasibility of the problem, when the problem is infeasible. same notation when a, b,andc are themselves (column) vectors, We obtain the speedup by exploiting special structure in the in which case (a, b, c) is the column vector obtained by stacking FOP to compute the search direction in each iteration of an a on top of b on top of c. interior-point method. The computational effort of our method scales linearly with M,thenumberofcontactpoints;fora II. GRASP FORCE OPTIMIZATION PROBLEM generic SDP or SOCP, however, the computational effort grows 3 The rigid object is grasped at M contact points, which have as M .Ourmethodismuchfasterforsmallproblemsaswell. positions p(i) R3 (in the global coordinate system), for i = For a typical grasping problem with M =5 contact points, 1,...,M.Wewilluseapointcontactwithfrictionmodel.(In∈ our method solves the FOP in around 400 µs,ona3GHz Section IX-C, however, we explain how our methods can be Pentium IV, with a not particularly optimized C++ implemen- extended to handle other friction models, e.g., a soft contact tation. This means that 2500 FOPs can be solved in 1 s. By point that can exert a torque on the object.) The force applied at exploiting warm-start techniques, the typical time to solve a acontactpointp(i) will be denoted by f (i) R3 ,andisgiven FOP drops to around 200 µs,manyhundredsoftimesfaster in a local coordinate system, with x-axis and∈ y-axis tangent than generic SDP or SOCP solvers. to the object surface at p(i),andz-axis normal to the object It is difficult to make a direct speed comparison between our surface at p(i) and pointing inward. We denote its components as method and those of Buss et al. [25] and Helmke et al. [27], f (i) =(f (i),f(i) ,f(i)).Thus,f (i) is the normal (inward) force since these methods have different stopping criteria, not based x y z z (i) (i) (i) on an explicit dual bound, and the run time depends very applied at contact point p ,and(fx ,fy ) is the tangential (i) much on the details of the implementation. In fact, many of the force applied at contact point p . methods in this paper can be used in both of these algorithms. For example, our dual-based stopping criterion is easily used in A. Friction Cone Constraints both algorithms; moreover, our method for a fast solution of the The point contact friction model requires the contact forces linear equations that must be solved in each step can be used to satisfy the friction cone constraints to speed up the iterations in the Dikin method as well. In any (i) (i) (i)2 (i)2 (i) case, there is not much room for significant speed improvement (f ,f ) = fx + fy µ f ,i=1,...,M # x y # ≤ i z over our method, by any other method at all, since our method % (1) (i) requires around eight or so iterations, each of which entails a where µi > 0 is the friction coefficient at contact point p . few hundred M floating point operations. To put this in context, This constraint states that the magnitude of the tangential force

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. BOYD AND WEGBREIT: FAST COMPUTATION OF OPTIMAL CONTACT FORCES 1119 does not exceed the friction coefficient times the normal force. gives the total wrench on the object from all the contact forces. (i) (In particular, it implies that the normal forces fz must be Thus, we can write the equilibrium conditions (4) and (5) as nonnegative.) The friction cone constraints are second-order Af + ωext =0. (6) cone constraints [29, Sec. 4.4.2] [28]. We introduce the friction 3 cones K1 ,...,KM R ,definedas This is a set of six linear equations in the 3M contact forces f. ⊆ 3 2 2 Ki = x R x1 + x2 µi x3 ,i=1,...,M. C. Contact Force Constraints ∈ ≤ & ' ( ' % (2) In many practical problems, there are further constraints on ' Using this notation,' we can express the friction cone constraints the contact forces due to actuator limits, kinematic constraints, (1) compactly as and other limits, which we can describe as

(i) other f Ki ,i=1,...,M. (3) f . ∈ ∈C B. Equilibrium Constraints These constraints are typically convex, and often polyhedral, i.e., other is described by a set of linear equalities and inequali- (i) Let Q SO(3) be the 3 3orthogonalmatrixthattrans- ties onC the contact forces. In the main development of this paper, ∈ × (i) forms forces in the local coordinate system at p into the global we will ignore these constraints; but we will make occasional coordinate system. Thus, the force on the object due to the con- comments about how our methods can be extended to handle (i) (i) (i) tact force at p is Q f (in the global coordinate system). these constraints. The force equilibrium condition is Q(1)f (1) + + Q(M )f (M ) + f ext =0 (4) D. Force Optimization Problem ··· (i) where f ext R3 is the total external force that acts on the object The FOP is to find a set of contact force vectors f ’s that (in the global∈ coordinate system). are as small as possible, while satisfying the friction cone con- The torque applied to the object by the force at contact point straints (3) and the force and torque equilibrium conditions (6). p(i) is given by We will measure the size of the set of contact forces by the maximum magnitude of the M contact forces: p(i) Q(i)f (i) ⊗ F max = max f (1) ,..., f (M ) (in the global coordinate system). The torque equilibrium {# # # #} (i)2 (i)2 (i)2 condition is = maxi=1,...,M fx + fy + fz . (1) (1) (1) (M ) (M ) (M ) ext p Q f + + p Q f + τ =0 Our problem can be expressed% as ⊗ ··· ⊗ where τ ext R3 is the total external torque that acts on the minimize F max ∈ (i) object (in the global coordinate system). Then, we can write the subject to f Ki,i=1,...,M (7) torque equilibrium condition as Af +∈ωext =0. S(1)Q(1)f (1) + + S(M )Q(M )f (M ) + τ ext =0 (5) The optimization variables are the contact forces, i.e., f R3M ; ··· the problem data are (which describe the geometry of∈ the con- where A tact points), ωext (the externally applied wrench), and K (the (i) (i) i 0 pz py friction cones, which depend on problem data µi, the friction (i) (i) − (i) S = pz 0 px skew(3) coefficients). We note for future reference that A has rank 6, pro-  (i) (i) −  ∈ py px 0 vided M 3,andthecontactpointsarenotcollinear(which  −  we assume).≥ is the skew-symmetric matrix that satisfies S(i)x = p(i) x. ⊗ The problem (7) is convex, since the objective is a convex We now introduce some more compact notation for the (although nondifferentiable) function of f,andtheconstraints equilibrium constraints. We define the contact force vector consist of linear equalities and (convex) cone constraints. The f R3M as ∈ problem remains convex if we add additional (convex) force (1) (M ) constraint f other. f =(f ,...,f ) ∈C the vector of all the contact forces. We define contact matrices E. Conic Formulation 6 3 Ai R × as ∈ The FOP (7) can be expressed in conic form, indeed, as an Q(i) A = ,i=1,...,M SOCP, by introducing a new scalar variable F that bounds the i S(i)Q(i) ) * magnitude of the contact forces: 6 3M and the (overall) contact matrix A =[A1 AM ] R × . minimize F ··· ∈ We collect the external force and torque into a single external subject to (f (i),F) K, i =1,...,M (8) wrench ωext =(f ext,τext) R6 .NotethatA(i)f gives the f (i) K ∈,i=1,...,M ∈ i wrench on the object due to the force at contact point p(i) and Af Af +∈ωext =0.

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. 1120 IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007

Here, K denotes the standard second-order cone in R4 the wrenches all have unit norm, and are maximally equidistant on the unit sphere. 4 2 2 2 T K = x R x + x + x x4 We start with the 7 7 matrix Z = I (1/7)11 ,where ∈ 1 2 3 ≤ × − & ' ( 1 R7 is the vector with all entries one. This matrix has ' % ∈ so (f (i),F) K means ' value 6/7 on its diagonal and 1/7 for each off-diagonal en- i ' ∈ try. Its eigenvalues are 0 (with− multiplicity 1) and 1 with mul- (i)2 (i)2 (i)2 T fx + fy + fz F. tiplicity 6. Therefore, we can factor it as V V = Z,where ≤ 6 7 % V R × (for example, using the eigenvalue decomposition). In the problem (8), there are 3M +1(scalar) variables: the con- ∈ T 3M Let v1 ,...,v7 denote the seven columns of V .FromV V = Z, tact forces f R and F ,thenewscalarvariableintroduced T ∈ we conclude that vi = 6/7,andthatvi vj = 1/7 for to bound the maximum force magnitude. # # 1 − i = j,so (v ,v )=cos− ( 1/6) 100◦ for i = j.Wecan Although we will not use the SDP formulation, we mention * * i j +− ≈ * take ω = 7/6v . that it is readily derived from the conic formulation of the FOP, i i There are several ways to express a general ω as a nonnegative by expressing the second-order cone constraints as linear matrix + linear combination of ω ,...,ω .Onemethodistofirstexpress inequalities (LMIs). For example, we can express f (i) K as 1 K i ω as a linear combination the LMI ∈

(i) (i) ω = βˆ1 ω1 + + βˆK ωK µi fz 0 fx ··· (i) (i) 0 µi fz fy 0 for example, as βˆ =[ω1 ωK ]†ω,where denotes pseudo-  (i) (i) (i)  ) ··· † fx fy µi fz inverse. Then, we take β = βˆ + ρλ,whereλ is from (10), and ρ   where denotes matrix inequality. This gives the SDP (or LMI) is chosen large enough such that all entries of β are nonnegative. formulation) of the FOP [26]. A more compact formulation, The second method we describe involves a set of 12 wrenches, which uses only 2 2 LMIs, is given in [27]. but has the advantage that no computation is involved in ex- × pressing a general wrench as a positive linear combination of F. Fo rc e C l o s u re the given wrenches. The wrenches are 6 Force closure occurs if, for every wrench ω R , there exists e1 ,..., e6 ± ± asetofcontactforcesf (i) K for which Af∈+ ω =0.Wecan i where e is the ith unit vector in R6 .Thesumofthesewrenches describe this condition in terms∈ of cones. Define i is zero, so the condition (10) holds with λi =1.Supposewe (i) W = Af f Ki (9) solve the 12 associated FOPs, which are all feasible (so force {− | ∈ } + closure occurs). Let fi be an optimal contact force for wrench which is the cone of wrenches that can be resisted. Force closure ext ω = e ,andletf − be an optimal contact force for wrench is equivalent to W = R6 . i i ωext = e .Oncewehavecomputedthissetof12contact We can determine force closure by solving a set of FOPs. Let i forces, we− can very easily construct a feasible contact force ω ,...,ω R6 be a set of wrenches with 0 in the interior of 1 K ∈ vector for any wrench ω R6 ,asfollows: their convex hull, i.e., there exist positive λ1 ,...,λK such that ∈ + + f =(ω1 )+ f1 +(ω1 ) f1− + +(ω6 )+ f6 +(ω6 ) f6− λ1 ω1 + + λK ωK =0. (10) − ··· − ··· (11) (The minimum possible value of K is 7.) Force closure occurs where (u)+ = max 0,u and (u) = max 0, u are the pos- { } − { − } if and only if each of these wrenches can be resisted, i.e., the itive and negative parts of u R,respectively,andhere,ωi de- associated K FOPs are all feasible. notes the ith component of ∈ω.Thus,oncewehavesolvedthe If any of these FOPs is infeasible, then evidently force closure 12 FOPs, we can instantly generate a feasible (but suboptimal) does not occur. On the other hand, suppose that each of these K contact force vector for any external wrench, using the formula 3M FOPs is feasible, with optimal force vectors f1 ,...,fK R . (11) that describes a piecewise-linear mapping from ω into f, Given any ω R6 ,wecanwriteitasanonnegativelinear∈ with 12 regions, i.e., the orthants in R6 . ∈ combination of the vectors ω1 ,...,ωK : As a consequence, we find that the minimum grasping force required to resist the wrench ω is no more than G ω 1 ,where ω = β1 ω1 + + βK ωK ,β1 ,...,βK 0. # # ··· ≥ G is the maximum of the optimal value of the 12 FOPs, and It follows that the force vector ω = ω + + ω .Thus,thenumberG,themaximum # #1 | 1 | ··· | 6 | of the optimal grasping forces for the wrenches ei ,givesa f = β1 f1 + + βK fK ± ··· quantitative measure of force closure. is feasible, i.e., satisfies the friction cone constraints and resists There are several other uses of the formula (11). Suppose we the external wrench ω. have computed an optimal contact force vector f for a wrench We describe two simple methods for constructing a set of ω,andwewishtocomputeacontactforcevectorf˜for a nearby wrenches with the required property. The first method constructs wrench ω˜. (In fact, ω˜ does not have to be close to ω.) To do aminimalset,i.e.,asetofsevenwrenches.Inparticular,it this, we use the formula (11) to find a feasible contact force constructs the wrenches as the vertices of a regular simplex in vector δf that resists the external wrench ω˜ ω.Then,the R6 ,centeredat0,sothesumofthewrenchesiszero.Moreover, contact force vector f + δf satisfies the cone constraints,− and

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. BOYD AND WEGBREIT: FAST COMPUTATION OF OPTIMAL CONTACT FORCES 1121

resists the wrench ω˜.Thiscontactforcevectorcanbecomputed have K∗ = K;thedualfrictionconesaregivenby almost instantly; moreover, if ω and ω˜ are not too far from each 3 2 2 other, the force vector will not be too suboptimal. K∗ = y R y + y (1/µ )y . (12) i ∈ 1 2 ≤ i 3 We mention one important feature of (11). The original FOP & ' ( ' % is invariant under a change of the global geometric coordinate (This follows from the general' fact that, for any cone K and ' T system. If we change the global coordinate system, we will any nonsingular matrix C,wehave(CK)∗ = C− K∗.) We can change ω and the matrices Ai,andtherefore,thebasicFOPalso. considerably simplify the basic Lagrange dual problem men- But the solution of this FOP will be the same as the solution tioned earlier, by eliminating all variables except ν R6 .Since ∈ of the original FOP. This property does not hold for (11), i.e., a (u ,v ) K∗ = K is the same as u v ,wecaneliminate i i ∈ # i #≤ i change of the global coordinate system will, in general, change the variables vi and express the dual as the force vector f computed from (11). (This is obvious since T ext (11) depends on an expansion of the external wrench in a specific maximize ν ω basis.) We have found that (11) gives good results when the zero subject to zi K∗,i=1,...,M ∈ i coordinate of the global coordinate system is chosen to be near M u 1 i=1 # i#≤ the contact point locations, e.g., their average. AT ν = u + z ,i=1,...,M. /i i i T III. DUAL PROBLEM AND INFEASIBILITY CONDITIONS We now eliminate the variables ui ,usingui = Ai ν zi ,to obtain − A. Dual Force Optimization Problem maximize νT ωext In this section, we derive a dual problem for the FOP (7). (For subject to zi Ki∗,i=1,...,M (13) ageneralreferenceonLagrangeduality,see[29,Ch.5].)We M T∈ 3 i=1 Ai ν zi 1. introduce Lagrange multiplier vectors zi R for the friction # − #≤ ∈ 4 cone constraints and Lagrange multiplier vectors (ui ,vi ) R Finally, we eliminate/ the variables zi.Theoptimalzi in the 3 ∈ T with ui R and vi R,fortheconstraintsinvolvingK,and aforementioned problem is the one that minimizes Ai ν zi 6 ∈ ∈ T# − # ν R for the equality constraint. The Lagrangian is then over zi K∗,i.e.,the(Euclidean)projectionofA ν onto K∗: ∈ ∈ i i i L(f,F,z,u,v,ν) z = P (AT ν) i K i∗ i M M T (i) T (i) and the associated minimum value is = F zi f (ui ,vi ) (f ,F) − − T T i=1 i=1 minzi K ∗ Ai ν zi = dist(Ai ν,Ki∗) , , ∈ i # − # T ext + ν (Af + ω ) T the distance from Ai ν to Ki∗.Thus,wecanwritethedualFOP M M as = 1 v F (z + u AT ν)T f (i) + νT ωext. − i − i i − i maximize νT ωext - i=1 . i=1 M T (14) , , subject to i=1 dist(Ai ν,Ki∗) 1 To obtain the dual function, we minimize L over the primal ≤ 6 variables F and f. When we minimize L over F ,wefindthat with variables ν R . / M We can give an∈ explicit formula for the projection of a point the minimum is ,unless i=1 vi =1.MinimizingL over (i) −∞ T 2 yields unless for .The (x, y),withx R and y R,ontotheconeKi∗: f Ai ν = ui + zi i =1,...,M ∈ ∈ dual function−∞ is, therefore, given/ by (x, y),yµ x ≥ i# # g(z,u,v,ν) PK (x, y)= β(x, µi x ), (1/µi ) x y µi x i∗  0,y# # − (1#/µ#≤) x ≤ # # = inf L(f,F,z,u,v,ν)  ≤− i # # f, F where  νT ωext, M v =1,AT ν = u + z ,i=1,...,M = i=1 i i i i µiy + x , otherwise. β = 2# # . & −∞ / (1 + µi ) x Thus, we can write the dual of the FOP (see, e.g., [29, Sec. 5.9]) # # The corresponding distance is given by as T ext maximize ν ω dist((x, y),Ki∗) subject to zi Ki∗,i=1,...,M ∈ 0,yµi x (ui ,vi ) K∗,i=1,...,M = (µ x y)/ 1+µ2 , (1≥/µ#) #x y µ x M ∈  i # #− i − i # #≤ ≤ i# # i=1 vi =1 (x, y) ,y(1/µ ) x . T  i A ν = ui + zi ,i=1,...,M # # + ≤− # # /i  (15) with variables z, u, v,andν; K∗ denotes the dual of the standard 4 T second-order cone in R and Ki∗ denotes the dual of the friction This gives us an explicit formula for dist(Ai ν,Ki∗),sothedual cone Ki .Thestandardsecond-orderconeisself-dual,i.e.,we (14) is completely explicit.

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. 1122 IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007

The dual FOP (14) satisfies the following properties. T (i) T T (i) since Ai ν Ki∗ and f Ki imply (Ai ν) f 0. 1) Weak duality: Suppose ν is dual feasible, i.e., The infeasibility∈ conditions∈ (18) are closely related≥ to the M T i=1 dist(Ai ν,Ki∗) 1.Then,thedualobjectivevalue dual FOP (14): if ν satisfies the alternative constraints (18), νT ωext is a lower bound≤ on F ' ,theoptimalvalueofthe / then for any α>0, αν is feasible for the dual problem (14), (primal) FOP (7). T T ext since dist(Ai (αν),Ki∗)=0.Therefore,αν ω is a lower 2) Strong duality: Suppose the primal FOP (7) is strictly bound on F ' ,foranyα>0.Sincethiscanbemadearbitrarily ext feasible, i.e., there exists (f,F) with Af + ω =0, large, we must have F ' = ,i.e.,theFOP(7)isinfeasible. (i) (i) (f ,F) intK,andf int Ki .(int S denotes the We can interpret the infeasibility∞ conditions (18) in terms ∈ ∈ ' interior of the set S.) Then, there exists a dual feasible ν of energy supplied to the object by the external wrench, when 'T ext ' ' for which ν ω = F .(Infact,ν is optimal for the the object is displaced by a small amount. We think of the dual FOP (14).) dual variable ν R6 as representing a small displacement and T ext Since the dual objective ν ω and the dual constraint rotation of the∈ object; νT ωext > 0 means that some positive K T function i=1 dist(Ai ν,Ki∗) are both homogeneous, we energy is supplied to the object by the external force. The vectors K T can scale any ν for which dist(A ν,K∗) =0so that T / i=1 i i Ai ν give the displacement, to first order, of the contact points, in K T * T (i) dist(A ν,K∗)=1.Thisgivesthelowerbound the local coordinates. Since A ν and f are in dual cones, we i=1 i i / i have (AT ν)T f (i) 0 for any feasible set of forces. This means / νT ωext i ≥ F dual = F ' (16) that if the contact forces satisfy the friction cone constraints, K T ≤ i=1 dist(Ai ν,Ki∗) each one provides zero or positive energy to the object. But the K T total energy supplied to the object, by the external wrench and valid for i=1 dist(A/i ν,Ki∗) =0.(Thislowerboundisinter- esting only when νT ωext > 0; otherwise,* it gives a worse lower the contact forces, must be zero. We conclude that no feasible bound than/ the trivial one 0 F ' .) contact forces exist. ≤ B. Sensitivity Interpretation D. Force Nonclosure The optimal dual variable ν' is useful as a measure of the sensitivity of the optimal grasping force with respect to changes We can now give similar necessary and sufficient conditions in the external wrench. Assuming that F ' is a differentiable for the absence of force closure. This is the condition that the function of ωext (which is not always the case), we have FOP is infeasible for some externalwrench;bytheearlierresults, this is equivalent to ' ∂F ' ext = νi . ∂ω i T Ai ν Ki∗,i=1,...,M, ν =0. (19) ' ∈ * For example, ν1 gives us (approximately) the change in min- imum required grasping force, per Newton of increase in the Given such a ν,wecaneasilyconstructanexternalwrenchthat x-component of the externally applied force. Thus, if ν' is cannot be resisted; for example, ωext = ν.Moregenerally,aν large, small changes in externally applied wrench can lead to that satisfies the no-force-closure conditions (19) gives us an large changes in the minimum required grasping force. entire half-plane of wrenches that cannot be resisted, i.e., any ωext with νT ωext > 0. C. Infeasibility Conditions We also note that ν can be interpreted as a tangent plane, at 0, to the cone W of resistable− wrenches [defined in (9)], i.e., for The FOP (7) is feasible when there exists a force vector f T that satisfies the friction cone and equilibrium constraints, i.e., each nonzero ω W ,wehaveν ω 0.Indeed, ν W ∗,the dual of the cone of∈ resistable wrenches;≤ this establishes− ∈ that force (i) ext f Ki ,i=1,...,M, Af + ω =0 (17) closure fails, since force closure is equivalent to W ∗ = 0 . ∈ { } asetofM second-order cone constraints and six linear equality constraints. The associated set of alternative constraints give IV. BARRIER SUBPROBLEM the conditions under which (17) is infeasible: In this section, and then, next, we describe and analyze the T T ext A ν K∗,i=1,...,M, ν ω > 0 (18) barrier subproblem associated with the FOP (8), and Newton’s i ∈ i method, which is used to solve it. This subproblem arises di- asetofM second-order cone constraints for the dual variable rectly in interior-point methods such as primal barrier methods, ν R6 .Theconditions(17)and(18)arestrongalternatives:for including the algorithm we will describe, and also indirectly in any∈ problem data, exactly one of them is feasible [29, Sec. 5.9.4]. all others, e.g., primal-dual methods. The Dikin search direc- Thus, feasibility of either set of conditions implies infeasibility tion [25] can also be expressed in terms of the barrier subprob- of the other set of conditions. lem: it is the Newton search direction for the subproblem, in the It is easy to verify weak duality, i.e., that (17) and (18) cannot limit t (and suitably scaled). both be feasible. If they were both feasible, say with f and ν, The→∞ barrier subproblem is then we would have M T ext T T (i) T ext minimize 0=ν (Af + ω )= (Ai ν) f + ν ω > 0 tF + φ ext (20) i=1 subject to Af + ω =0 ,

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. BOYD AND WEGBREIT: FAST COMPUTATION OF OPTIMAL CONTACT FORCES 1123 where t>0 is a parameter and φ is the log barrier for the cone Here, r(i) R3 is the portion of the dual residual associated constraints. The log barrier is given by with contact∈ point i.Wewillgivemoreexplicitformulasfor

M these in Section IV-D. (i) The optimality conditions for the barrier subproblem (20) can φ(f,F)= φi(f ,F) i=1 be expressed in terms of the primal and dual residuals as , where rp =0,rd =0.

φ (f (i),F)= log(F 2 f (i)2 f (i)2 f (i)2) (See, e.g., [Ch. 10].) i − − x − y − z log(µ2 f (i)2 f (i)2 f (i)2) B. Dual Feasible Points From Subproblem Solution − i z − x − y ' ' ' if (f (i),F) satisfies the cone constraints strictly, i.e., If (f ,F ,ν ) are optimal for the barrier subproblem, then ν' /t is feasible for the dual FOP (14). The associated dual (f(i),F) intK, f(i) intK (21) objective value is ∈ ∈ i ν'T ωext 4M and + otherwise. The log barrier is a smooth convex function, = F ' . so the∞ subproblem (21) is a smooth convex problem. (For more t − t on log barriers for convex cones, see [29] and [31].) This can be shown by direct computation, or from general facts Unless stated otherwise, we will assume that the subprob- about conic problems and logarithmic barriers, since here we lem is feasible, i.e., there exists a point (f,F) that satisfies have 2M second-order cones, each with a θ-value of 2 (see [Sec. Af + ωext =0and (21). This assumption is slightly more than 11.6], [31]). It follows that the solution of the barrier subproblem assuming that the original FOP (8) is feasible; it means that is at most 4M/t suboptimal for the FOP (8). the original FOP is strictly feasible. This assumption can be simplified as follows: there exists f R3M that satisfies C. Newton Method ∈ ext (i) Af + ω =0,fintKi ,i=1,...,M. We use an infeasible start Newton method to solve the barrier ∈ subproblem (20) [Sec. 10.3]. The Newton method starts with, (We can always choose F large enough to satisfy the other cone and maintains, (f,F,ν) R3M R R6 ,with(f,F) strictly constraints strictly.) satisfying the cone constraints∈ [i.e.,× (21)],× but not necessarily the Before getting into details, we mention the most important equality constraints Af + ωext =0.Withinafinitenumberof facts about the barrier subproblem (20). First, it is a convex steps, however, the equality constraints become satisfied, and optimization problem, with smooth objective and equality con- once satisfied, they remain satisfied at all subsequent iterations. straints, so it can be solved by Newton’s method. Second, it In particular, if the infeasible start Newton method is started at gives an approximate solution of the FOP (8): the solution (f,F,ν) with the equality constraints Af + ωext =0satisfied, (f,F) of the barrier subproblem is guaranteed to be at most the equality constraints will be satisfied at all future iterates. 4M/t-suboptimal for the original FOP (8). At each iteration of the Newton method, we compute the Newton step (df , d F , d ν ) at the current point (f,F,ν).Wethen A. Barrier Subproblem Optimality Conditions carry out a backtracking line search to find a step length γ, based on the merit function r 2 + r 2 .Thebacktracking Suppose (f,F) strictly satisfies the cone constraints, i.e., (21) d p line search algorithm chooses# γ#= βk#,where# β (0, 1) is an holds. The primal residual is algorithm parameter, and k is the smallest nonnegative∈ integer ext 6 rp = Af + ω R for which ∈ 1/2 1/2 rˆ 2 + rˆ 2 (1 αγ) r 2 + r 2 which is the error in the equilibrium force and torque balance # d # # p # ≤ − # d # # p # condition. The dual residual is holds,5 where rˆd and6 rˆp are the primal5 and dual6 residuals T evaluated at the tentative point (f + γdf,F + γdF,ν + γdν), rf f φ(f,F)+A ν 3M +1 rd = = ∇ R and α (0, 1/2) is another algorithm parameter. (In particu- rF t + ∂φ(f,F)/∂F ∈ ) * ) * lar, we∈ require that (f + γdf,F + γdF) strictly satisfies the where ν R6 is the Lagrange multiplier associated with the cone constraints.) The parameters must satisfy 0 <β<1 and ∈ equality constraints. Here, r R3M is the component of the 0 <α<1/2.Commonchoicesfortheparametersareβ =1/2, f ∈ dual residual associated with the force vector f,andr R is α =0.1.(TheNewtonmethodisfairlyinsensitivetothechoice F ∈ the component of the dual residual associated with F . of these parameters.) We can further divide rf into components corresponding to After the line search, we update the new point as the individual contact points, as f := f + γdf, F := F + γdF, ν:= ν + γdν. (1) (1) T r f (1) φ1 (f ,F)+A1 ν ∇ The Newton method is terminated when the norm of the resid- . . rf = . = . .    .  ual is small enough, or when some other exit criterion is sat- (M ) (M ) T r ( M ) φ (f ,F)+A ν isfied. Once a full Newton step is taken (i.e., the line search  ∇f M M     

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. 1124 IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007 gives γ =1), the primal residual becomes zero, and remains D. Barrier Gradient and Hessian Formulas zero (to within numerical accuracy) from that step on; see In this section, we give explicit expressions for the gradient [29, Sec. 10.3.2]. and Hessian of the log barrier, which are needed to define the The main effort of the algorithm is in computing the Newton dual residuals, and the coefficient matrix in the KKT system step 3M +7,whichisgivenbythesolutionof (df , d F , d ν ) R (23). the Karush–Kuhn–Tucker∈ (KKT) system The log barrier φ is a sum of 2M terms, each of which is ∂2 φ ∂2 φ the negative log of a quadratic form. So, we first give a general AT ∂f2 ∂f∂F df rf formula for the gradient and Hessian of the function   ∂2 φ ∂2 φ dF = r 0   −  F  ψ(u)= log(uT Pu)  2   ∂F∂f ∂F  dν rp −       T  A 00     where P is a symmetric matrix. (We assume that u Pu > 0.)   asetof3M +7equations in 3M +7variables. Since the log We have barrier is a sum of functions of each f (i),thesecondderivative 2 ψ = Pu appearing in the upper left entry of the KKT matrix is block ∇ −uT Pu diagonal, so we can express the KKT system as 2 2 2 2 T H q AT ψ = P + (Pu)(Pu) . (24) 1 1 1 df 1 r1 ∇ −uT Pu uT Pu 7 8 ......  . . .   .   .  Using this formula (or by direct differentiation), we have T = (22) HM qM A df M rM  M    −   (i)  T T      ∂φi 2fx  q1 qM HF 0   dF   rF  =  ···      (i) 2 (i)2 (i)2 (i)2       ∂fx F fx fy fz  A1 AM 00  dν   rp  − − −  ···      where 2f (i) + x ∂2 φ 2 (i)2 (i)2 (i)2 i 3 3 µi fz fx fy Hi = (i)2 R × − − ∂f ∈ (i) =(ai + bi )fx ∂2 φ H = R F ∂F2 ∈ where we define 2 ∂ φi 3 2 qi = R . (23) ai = (i) 2 (i)2 (i)2 (i)2 ∂f ∂F ∈ F fx fy fz − − − Explicit formulas for these will be given in the next section; for 2 bi = . now, we note the sparsity pattern of the KKT system, which 2 (i)2 (i)2 (i)2 µ fz fx fy consists of a block diagonal part, with M 3 3blocks(each i − − × associated with a contact point), bordered by seven dense rows We have similar expressions for the partial derivative with re- and columns (corresponding to the objective variable F and dual (i) (i) (i) spect to fy ,withfx changed to fy in the numerators. The variable ν). (i) partial derivative with respect to f is The infeasible start Newton method always converges to the z optimal point for the barrier subproblem, provided the original (i) ∂φi 2fz FOP is strictly feasible. When the original FOP problem is = (i) 2 (i)2 (i)2 (i)2 ∂fz F fx fy fz not strictly feasible, the barrier subproblem is not feasible, and − − − the Newton method does not converge; the residuals do not 2 (i) 2µ fz converge to zero. Moreover, the Newton method cannot take i 2 (i)2 (i)2 (i)2 − µ fz fx fy astepsizeγ =1(because if it did, the next iterate would be i − − strictly feasible). 2 (i) =(ai µi bi)fz . The convergence of Newton’s method is quadratic, so ter- − minal convergence is extremely fast. For the same reason, if Thus, the gradient is given by the method is started at an initial point with primal and dual (i) residuals not too large, convergence to high accuracy can be ob- (ai + bi )fx (i) (i) tained within a few steps. The total number of Newton steps can ( i ) φi(f ,F)= (a + b )f . ∇f  i i y  be bounded using the theory of self-concordance [31], but the 2 (i) (ai µi bi )fz bounds obtained are usually far larger than the actual number  −  required in practice, which is often fewer than ten, and rarely The partial derivative of φ with respect to F is more than a few tens. When the starting point is good, i.e., the M M starting residuals are small, convergence typically occurs in just ∂φ 2F = = ai F. ∂F − 2 (i)2 (i)2 (i)2 − a few steps. i=1 F fx fy fz -i=1 . , − − − ,

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. BOYD AND WEGBREIT: FAST COMPUTATION OF OPTIMAL CONTACT FORCES 1125

Therefore, the dual residuals can be explicitly expressed as their definiteness properties are known ahead of time. The lead- (i) ing 3M 3M block is a block diagonal, with 3 3blocksthat (ai + bi )fx are positive× definite, and so, can be inverted very× efficiently. A (i) T ri = (a + b )f + A ν, i =1,...,M  i i y  i basic elimination method (see, e.g., [Appendix C, 29]) applied 2 (i) (a µ b )fz to this block yields a 7 7setoflinearequationstosolve.This i − i i   reduced system of linear× equations is indefinite, but it contains a rF = t (a1 + + aM )F. (25) − ··· 6 6 subblock that is positive definite, so a further elimination Now, we work out the blocks in the barrier Hessian 2 φ, step× can be taken, using a Cholesky factorization to invert the ∇ using the notation in (23). Using the general formula (24), we positive definite subblock. have Before giving the details of our elimination method, we com- ment on how it differs from using a good sparse solver to com- H = a I + a2 f (i)f (i)T + b diag(1, 1, µ2 ) i i i i − i pute the Newton step. Such a method decomposes the KKT (i) (i) T matrix as PLDLT P T ,whereP is a permutation matrix cho- fx fx 2 (i) (i) sen to reduce fill-in and preserve numerical stability, L is unit + bi  fy   fy  . lower triangular, and D is diagonal. It is very likely that the 2 (i) 2 (i) permutation found will be good enough to yield an algorithm  µi fz   µi fz   −   −  that solves the KKT system in O(M) flops, just like our method. The vectors q that form the last row and column of the Hessian i Ours, however, is completely explicit, and also exploits a small are given by amount of further structure, such as the easy invertibility of the 2 (i) qi = ai Ff ,i=1,...,M. Cholesky factors. Moreover, our method involves no condition- − als, which makes it faster (and simpler) than a generic sparse Finally, we have PLDLT P T method. And finally, since our method uses an ex- ∂2 φ plicit elimination ordering, it expends no effort in discovering a H = F ∂F2 good elimination ordering. M A. First Elimination Step = ( a + a2 F 2 ) − i i i=1 Our first step is to eliminate the variables df from (22). From , i M 2 (i) 2 T F + f Hi df i + qi dF + Ai dν = ri = a # # − i F 2 f (i) 2 i=1 we obtain , −# # M 1 T a2 df i = Hi− (ri + qi dF + Ai dν). (26) = i (F 2 + f (i) 2 ). − 2 # # Combining this with i=1 , M V. E FFICIENT NEWTON STEP COMPUTATION qT df + H dF = r i i F − F i=1 In this section, we show how to compute the Newton step , (df , d F , d ν ),i.e.,solvetheequations(22),efficiently.These we obtain equations can be solved using standard methods for linear M T 1 T equations, such as Gaussian elimination. For example, we q H− (r + q dF + A dν)+H dF = r − i i i i i F − F T i=1 can compute an LDL factorization of the KKT matrix, and , then, find (df , d F , d ν ) by a back and forward substitution. which can be written as 3 2 The cost of this is around (1/3)(3M +7) +2(3M +7) M M 3 2 ≈ T 1 T 1 T 9M +63M flops (floating point operations). While this can H q H− q dF + q H− A dν − F − i i i i i i be fast for modest values of M (say, M smaller than 10 or - i=1 . -i=1 . so), we can exploit the special structure of the equations to , , M solve the equations far faster. The method we describe in T 1 = rF q H− ri . (27) this section requires around 350M flops, and in addition, in- − i i i=1 volves no conditionals (which are needed to pivot in the gen- , Now, we use eral case), leading to extremely fast execution time. For M large, our method is clearly far faster than a general method. M A df = r It is also faster for small values of M such as M =4,for i i − p 3 2 i=1 which (1/3)(3M +7) +2(3M +7) is around two and one- , half times 350M. and (26) to get Our method is based on a sequence of two elimination steps, M in which particular blocks of variables are eliminated, using 1 T Ai Hi− (ri + qi dF + Ai dν)=rp Cholesky factorizations to compute the required inverses since i=1 ,

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. 1126 IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007 which we write as system is solved by Cholesky factorization, the lack of pivoting

M M comes at no cost in numerical stability. 1 1 T To solve the reduced system (29) by this method, we proceed AiHi− qi dF + Ai Hi− Ai dν -i=1 . -i=1 . as follows. We first note that , , M T dF =(1/E11)(e1 E21 dν) 1 − = r A H− r . (28) p − i i i i=1 and substituting this into E21 dF + E22 dν = e2 ,weget , T Now, we write (27) and (28) as E22 (1/E11)E21E dν = e2 (1/E11)E21e1 . − 21 − T E E dF e Since5E11 < 0,thematrixontheleftispositivedefinite.There-6 11 21 = 1 (29) E21 E22 dν e2 fore, we can solve this set of equations using Cholesky factor- ) *) * ) * ization; we then find dF from the earlier equation. where M C. Summary and Flop Count T 1 E = H q H− q 11 − F − i i i We summarize our efficient method for computing the - i=1 . , Newton step. We compute the primal and dual residuals, as M well as the Hessian, i.e., , ,and ,usingtheformulas 1 Hi HF qi E21 = AiH− qi i given in Section IV-D. We compute Cholesky factors Li of Hi, i=1 T , i.e., lower triangular Li with Li Li = Hi .Wethencomputethe M 1 inverses of the Cholesky factors, i.e., Li− .(Themorestandard T 1 T 1 E22 = Ai Hi− Ai method would be to use forward substitution to compute L− y, i=1 , but here, there is a slight advantage to simply inverting these M matrices.) The total flop count for this step is less than around T 1 e1 = rF qi Hi− ri 50 M flops. − 1 T 1 1 i=1 Next, we compute L A , L q ,andL r ,atatotalcost , i− i i− i i− i M of 73 M flops. (Since the cost of multiplying a lower triangular 1 e = r A H− r . 3 3matrixbyavectorisnineflops.)Fromthese,wecompute 2 p − i i i × i=1 the data in the reduced system, i.e., E11, E21, E22, e1 ,ande2 . , For example, to compute E ,weexpressitas The reduced system (29) is a set of seven linear equations in 22 seven variables, which is easily solved to get dF and dν.Once M 1 T T 1 T these have been computed, we can find df i from (26). E22 = (Li− Ai ) (Li− Ai ). i=1 , B. Second Elimination Step This costs 126 M flops. There are 21 entries in the upper triangle of the matrices in the sum (their lower halves are the same The reduced system (29) also has a structure that can be as their upper halves); each of these is computed as an inner exploited, even though it is fully dense. As we will show, the product of two vectors in R3 ,whichcostsfiveflops.Thisgives matrix E is not positive definite (or negative definite), so we 5 21 =105flops for each term in the sum, plus 21 M flops to cannot use a Cholesky factorization of E (or E)tosolvethe add× the matrices. system. We must use an LDLT factorization,− or a factorization Forming E is the dominant cost in forming the reduced such as LDU that ignores the symmetry. Generic algorithms 22 system: forming E and e each costs 6 M flops; forming E for each of these require numerical pivoting, which complicates 11 1 21 and e each costs 31 Mflops.Thus,thetotalflopcounttoform (and slows) the algorithm. 2 the reduced system is around 200 M flops. The 1, 1 entry E is negative. This follows from the convexity 11 The reduced system is solved using the method described of φ,whichimpliesthattheHessianispositivedefinite; E 11 in Section V-B, which has a small cost, independent of M,of is a Schur complement of 2 φ,andtherefore,positive.Its2,2− around (1/3)73 +2 72 200 flops. Finally, df ’s are com- entry E is, however, positive∇ definite. (It is clearly positive i 22 puted from (26), at a× cost around≈ 31 Mflops.Alltogether,then, semidefinite; it is positive definite since A is full rank.) Thus, we can compute the Newton step in around 350 M plus a modest the matrix E is indefinite; it has one negative eigenvalue and six constant around 200 flops. positive eigenvalues. We can solve the reduced system (29) by eliminating the 1, 1 block, which will leave a 6 6systemof VI. BARRIER METHOD equations that is positive definite, and can therefore,× be solved by a Cholesky factorization. In a classic primal barrier method, the barrier subproblem (20) This approach does not reduce the flop count, when compared is solved for an increasing sequence of values of t.TheNewton to a generic LDLT factorization method; but it removes all method for each subproblem is initialized at the optimal so- conditionals and row/column permutations (i.e., pivoting), and lution of the previous one. A typical method for increasing therefore, yields a simpler (and faster) algorithm. Since the final the parameter t is to multiply it by a factor on the order of 10

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. BOYD AND WEGBREIT: FAST COMPUTATION OF OPTIMAL CONTACT FORCES 1127

(see, e.g., [29, Sec. 10.3]). This is repeated until 4M/t is smaller F max and the lower bound F dual is smaller than our tolerance. than the required tolerance. This basic barrier method is reliable, We have found good results with η =0.8. and can solve the FOP using on the order of 50 or so Newton Choosing a fixed value of t (as we do in the algorithm de- steps, provided the problem is feasible, and the starting point is scribed before) is a poor choice for solving general SOCPs, reasonably close to feasible. Since each Newton step can be car- especially when high accuracy is required, and the number of ried out very quickly, this is already an attractive algorithm. We constraints is large. But for the FOP, a fixed choice of t works will show, however, that a variation on the basic barrier method very well, at least for modest M (say, smaller than 100). If high can reliably solve the problem to a good enough guaranteed accuracy is needed, or M is large (say, over 100), a more con- relative accuracy (say, 1%)usingalmostanorderofmagnitude ventional schedule for t (as in a primal barrier method) would fewer Newton steps, i.e., between five and ten steps. likely perform better. When the FOP (8) is infeasible, the basic primal barrier We can modify the algorithm to stop with a guaranteed rel- method fails during the first barrier subproblem, which never ative tolerance instead of a guaranteed absolute tolerance by finds a feasible point (since no such point exists). Even when replacing the stopping criterion with the FOP is feasible, but nearly infeasible, the number of itera- max dual tions required can be much larger than the typical number. These F F − .rel. (30) problems can be handled by preceding the basic barrier method F dual ≤ with a special phase I method, designed to find a feasible point, In this case, we must ensure that the value of t used is large or establish the infeasibility of the FOP. We will address this enough to drive F max F dual small enough so that (30) will issue in Section VI-B. For now, we assume that a feasible, or eventually hold. This requires− nearly feasible, starting point is available. 4M t> dual . A. Custom Primal Barrier Method .relF Our basic method consists of solving the barrier subprob- To do this, we can adjust t during the algorithm as follows. lem, for a fixed value of t,usingNewton’smethod.Oncef We start with an absolute tolerance, as in the algorithm before, is feasible, we evaluate the current maximum force F max = chosen so that it is likely to be sufficiently small to work for max f (1) ,..., f (M ) using the current value of f,andthe the required relative tolerance. We terminate if the relative toler- current{# dual# bound#F dual#}[from (16)] using the current estimate ance is reached (in which case we are done), or when the absolute of ν,i.e.,ν¯/t,whereν¯ is the current value of the dual variable in tolerance is reached. In the latter case, we simply update t to be the Newton subproblem. We terminate if the current maximum 4M max force F is within a given tolerance of the current dual lower t = dual η.relF bound. given where F dual is the current value of the dual bound, and continue starting point (f,F,ν) that satisfies (21) until the relative stopping criterion (30) is satisfied (which must required tolerance .abs > 0 occur). parameter η (0, 1) Finally, we describe two modes of early termination for the ∈ Set t := 4M/(η.abs). algorithm that we will employ in Section VII. We are given two repeat target values Ltar and Utar.Weterminatethealgorithmonce 1. Compute Newton step (df , d F , d ν ) using the we have achieved either F max U (i.e., the maximum force ≤ tar methods described in Section V. is less than the upper target value) or F dual L (i.e., the ≥ tar 2. Line search and update. dual bound shows that the optimal maximum force is more than Choose a step size γ using backtracking (see Section IV-C). lower target value). 3. Update. (f,F,ν):=(f,F,ν)+γ(df , d F , d ν ). 4. if Af + ωext =0,evaluateF max and the dual B. Phase I Problem lower bound F dual (16). The custom primal barrier method described in the previous until F max F dual . . abs section will work when started from any infeasible point that We make− some observations≤ about this algorithm. First, the strictly satisfies the cone constraints, provided the original FOP algorithm exits with a guarantee (via the dual bound) that the is strictly feasible. If the starting point is far from feasibility, maximum force is no more than . -suboptimal, even though abs however, the algorithm can require a large number of iterations. Newton method may not have converged. Second, the exit con- When a good starting point is not available, it is more efficient dition must hold eventually; if it did not, Newton method con- to break the optimization up into two phases. In phase I, we verges to the optimal point for the barrier subproblem, at which determine a feasible point, i.e., an f that satisfies the constraints we have (17), or verify that the problem is infeasible, by exhibiting a max dual dual 4M feasible solution of the alternative inequalities (18). In phase II, F F F F = = η.abs <.abs − ≤ − t we use the custom barrier method to compute the optimal so- acontradiction.Theparameterη gives a “margin” that guaran- lution of the FOP (7), starting from the feasible point found in tees that if Newton’s method were to converge, the gap between phase I.

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. 1128 IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007

One choice for the phase I problem for the FOP is the solution of a set of equations with exactly the same sparsity structure encountered earlier in (23), i.e., block diagonal, with minimize s M 3 3blocks,borderedbysevendenserowsandcolumns. subject to f (i) + se K ,i=1,...,M 3 i (31) (The× particular coefficient matrices H , q ,andH ,however, Af + ωext ∈=0 i i F are different.) Therefore, the method described in Section V can s 1 ≥− be used to efficiently compute the Newton step for the phase I where the optimization variables are s R and f R3M ,and problem. ∈ ∈ e3 =(0, 0, 1).ThisphaseIproblemisalsoinconicform,i.e.,an When we solve the phase I problem with an absolute tolerance SOCP. The variable s (which is also the objective in the phase I .abs (which corresponds to t =2M/.abs), three outcomes can problem) can be interpreted as a fictitious force added to each of occur. We can terminate (early) having found a feasible point; (i) the normal forces fz for the purpose of satisfying the friction we can also terminate early, once the dual bound for the phase I cone constraint. But these fictitious forces do not appear in the problem shows the optimal value is positive. The third possibil- equilibrium condition Af + ωext =0.Thegoalistodrivethis ity is an ambiguous outcome, in which we have computed a set fictitious force to become zero (or negative); if we succeed, of forces that are feasible, when an additional normal contact the corresponding f is evidently feasible for the FOP (7). On force no more than .abs is added. This outcome is extremely the other hand, if the optimal value of the phase I problem is rare, and in any case, not a problem in practice. When a FOP is positive, it means that the original FOP (7) is infeasible. The just on the boundary between feasible and infeasible, it can be inequality s 1 is really not needed; it keeps the algorithm considered infeasible for all practical purposes. from finding≥− a point that has very negative s,atthecostofvery large forces. VII. SOLVING MULTIPLE FORCE OPTIMIZATION PROBLEMS For the phase I problem, we can easily construct a feasible In this section, we consider the problem of efficiently solving point from which to start the barrier method. First, we choose any ext afamilyofrelatedFOPs.Inthesimplestcase,wehavemultiple f that satisfies the equality constraints Af + ω ,forexample, FOPs to solve to some given accuracy. We can always solve them the least-norm solution one by one, using the method described before for each problem T T 1 ext f = A (AA )− ω . (32) instance. If the data for the FOPs differ sufficiently, treating each − problem as a brand new one is not a bad approach; but if the As an initial value of s,wecanchooseanyvaluethatsatisfies problem data are close, we can more efficiently solve them using warm-start techniques. Since the computational effort and time (i)2 (i)2 (i) s>max (1/µ ) fx + fy f i =1,...,M . i − z required to solve a FOP is measured by the number of Newton & ' ( % ' steps required, the goal is to solve FOP with fewer Newton steps The pair (f,s) is then feasible for the phase I' problem. Of course, ' than would be typically required. the force vector f in (32) need not satisfy the cone constraints; (i) for example, the normal forces fz can be negative. We also note A. Warm-Start that computing f from (32) can be done very efficiently, because the associated normal equations have exactly the same block Suppose we have solved a FOP, and then need to solve a new one with problem data that is “close,” i.e., the external sparsity pattern that allows us to use the methods of Section V. ext We use a barrier method to solve the phase I problem (31), wrench ω ,contactgeometrymatrixA,andcoefficientsof starting from this feasible point, and terminating as soon as friction µi are not far from those of the previous problem. In a s<0,sincethecorrespondingf is then strictly feasible for (7). warm-start technique, we simply use the previously computed In each outer iteration, we solve the problem force vector f as the starting point for Newton’s method, in the barrier subproblem associated with the new FOP to be solved, minimize ts + φ(f,s) (33) assuming that it satisfies the friction cone constraints for the subject to Af + ωext =0 new problem. (If it does not, we can simply add some extra where t>0 is a parameter and φ is the log barrier for the cone inward normal force to each contact force that does not satisfy constraints in the phase I problem: its cone constraint.) This force vector will not, in general, satisfy the equilibrium conditions for the new problem, but, assuming M that the new problem is strictly feasible, a feasible point will φ(f,s)= log(µ2 (f (i) + s)2 f (i)2 f (i)2) − i z − x − y eventually be found. i=1 , If the new FOP problem data are close enough to the previous (i) when f + se3 intKi and φ(f,s)= otherwise. The so- FOP problem data, the new problem can be solved in just a lution of this subproblem∈ is at most 2M/t∞ suboptimal for the few Newton steps. On the other hand, it is possible that this phase I problem (31), since here we have M cones, each with approach can require more iterations than a cold-start method, θ-value 2. i.e., solving the new problem as a new one, with a phase I We use Newton’s method to solve the smooth subproblem followed by a phase II. As an extreme example, if the new (33). We omit the details and formulas involved since they are problem is infeasible, then Newton method will simply fail to veryclosetotheonesencounteredearlier.Wenote,however,that converge. A very simple method for getting around this is to each step of Newton’s method for the phase I problem involves run the warm-start method for a fixed and small number of

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. BOYD AND WEGBREIT: FAST COMPUTATION OF OPTIMAL CONTACT FORCES 1129 iterations, such as 6; if a feasible point has not been found by problem of optimizing the contact points at which to grasp then, the warm-start process is abandoned, and the new problem agivenobject.Thiscanbedoneusinganoutersearchloop is solved using a cold-start. The hope is that the iterations saved that generates candidate manipulator positions and orientations; in the successful warm-start attempts more than offset the extra for each candidate, we find the resulting contact points, and iterations wasted in warm-start failures. then, solve the associated FOP to determine the minimum force Wenotethatgeneralinterior-pointmethodsare not well suited required to grasp the object. Here, our goal is to find the best to warm-start techniques. Our method handles warm-start well set of contact points, i.e., the contact points that (among those since it is a primal barrier method, with a fixed value of t,so considered) minimize the maximum grasping force. it inherits the (very good) warm-start properties of Newton’s Worst case short-circuiting works as follows. We maintain method. acurrentworstcasevalueofminimumgraspingforce(which As an example application, consider the problem of comput- we can set equal to zero number initially). We then solve each ing optimal grasping forces for an object, as it moves along FOP in turn, but we can terminate early if the algorithm finds a aknowntrajectory.Weassumethatthecontactpointsonthe feasible grasping force vector with F max smaller than the cur- object remain the same, but along the trajectory, the external rent worst case value. If the current FOP has minimum grasping wrench changes. The external wrench can include a gravity force larger than the current worst case value, its minimum term, as well as a term associated with accelerating the object. force becomes the new worst case value. This is called primal By choosing a global coordinate system tied to the object, the short-circuiting since we can terminate the FOP when the pri- matrix A remains constant along the trajectory, as the object mal value F max is less than a target value (i.e., the current worst moves, but the external wrench ωext varies. After computing case value). the optimal grasping forces at one time, we can use a warm- Best case short-circuiting is similar. Here, we maintain a start technique to rapidly compute the optimal grasping forces current best (smallest) value of minimum grasping force (which at the next sampled time. (In this special case with A not chang- we can set equal to initially). We solve each FOP in turn, ing, we can also use the method described in Section II-F. We terminating early if the∞ algorithm finds a dual lower bound F dual compute 12 wrenches that resist ei ;thereafter,wecanimme- on minimum grasping force that exceeds the current best case diately generate feasible, if not optimal,± contact force vectors at value. If the current FOP has minimum grasping force smaller each point along the trajectory.) Another example is computing than the current best case value, its minimum force becomes the the optimal grasping forces at the vertices of a box in wrench new best case value. This is called dual short-circuiting since space, i.e., all 64 combinations of high and low values of the six we terminate the FOP when the dual lower bound F dual exceeds components of ωext: atargetvalue(i.e.,thecurrentbestcasevalue).Notethatindual short-circuiting, it is possible to expend zero Newton steps on a ωext l ,u ,i=1,...,6. i ∈{i i } FOP. This occurs if the dual variable ν,fromthelastproblem We can solve the FOP for the average or nominal value of ωext, solved, certifies that the minimum grasping force for the current 1 problem will be worse than the best case already found. ωext = (l + u) 2 VIII. NUMERICAL EXAMPLES and then attempt to compute the optimal contact forces for each In this section, we give some numerical examples to illustrate vertex using a warm-start method. the algorithm and variations described before. All our examples use M =5contact points, determined by the position and the B. Short-Circuiting orientation of a particular manipulator with four fingers and Short-circuiting refers to early termination of the FOP algo- one palm contact point. (This, of course, does not affect the rithm, i.e., termination before the required relative or absolute algorithm; we are just pointing out that the sets of contact points tolerance has been reached. This can be done in two interesting are not chosen arbitrarily.) The external wrench is due to gravity. cases. In the first case, the goal is compute the worst case or Weusedthecoefficientoffriction µi =0.5 for all contact points. maximum value of the minimum grasping force over a set of We used the algorithm described before, with a phase I to FOPs. In the second case, the goal is compute the best case or determine feasibility and a phase II to minimize the maximum minimum value of the minimum grasping force over a set of grasping force. The algorithm parameters used are the ones men- FOPs. Of course, both of these problems can be solved by solv- tioned in the earlier algorithm description. The stopping crite- ing each FOP (possibly using warm-start techniques), and then, rion is 1% relative accuracy, i.e., .rel =0.01.(Thealgorithm simply computing the maximum (or minimum) of the optimal performs well for much higher precisions, but this accuracy is grasping forces. more than adequate for any practical use.) Examples of the first case include computing the maximum We solved 104 FOPs, generated using a collection of objects, grasping force that is required over a box in external wrench including bars, rods, boxes, and barbells, with various assumed space, or the maximum grasping force that is required to move weights and centers of gravity, and various grasp contact points. the object along a given trajectory. The second case arises in About 10% of the FOPs were infeasible. The average number optimizing the position and orientation of a manipulator, relative of Newton steps required to solve the FOP (or determine the to an object, using the minimum force required to grasp the infeasibility) is around 8, with a standard deviation around 2. object as the objective (see, e.g., [6]). This is the (nonconvex) The minimum number of Newton steps (over the 104 test cases)

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. 1130 IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007 was 3; the maximum was 16. The average number of Newton These examples show that both dual short-circuiting and steps in phase I was around 2. The average number of Newton warm-start techniques significantly reduce the number of steps required to certify infeasible FOPs was around 7. Newton steps required. With warm-start and primal short- Our implementation was coded in C++, and run on a 3 GHz circuiting, we are expending only 3 Newton steps per FOP. Pentium IV. Our implementation required around 50 µsper In our implementation, this corresponds to around 150 µsper Newton step, so the time to solve a FOP was around 400 µs. FOP. To illustrate warm-start and primal short-circuiting methods, we considered the problem of finding the worst case grasping IX. VARIATIONS AND EXTENSIONS force over a box of wrenches with each force and torque com- The methods presented in this paper can be extended in sev- ponent varying 25%.Wegenerated4000testproblems,and compared the average± number of Newton steps required, per eral ways, for example, by using a different objective function FOP, when all 64 vertices, plus the center, of the wrench box or a different contact point friction model. were solved using four methods. 1) Cold-start without short-circuiting. A. Other Objectives 2) Cold-start with short-circuiting. Instead of the maximum magnitude of the grasping forces, we 3) Warm-start without short-circuiting. can take as objective the sum of squares of the force magnitudes 4) Warm-start with short-circuiting. Our warm-start scheme ran for at most six steps, and reverted F sumsq = f (1) 2 + + f (M ) 2 # # ··· # # to a cold-start scheme if a feasible point had not been found or a sum of the force magnitudes, by then. (We counted the Newton steps wasted in warm-start failures in our averages.) The following table gives the average F sum = f (1) + + f (M ) number of Newton steps required, per FOP. # # ··· # # or the maximum normal force, Method Newton steps per FOP F norm,max = max f (1),...,f(M ) . { z z } Cold-start without S/C 10.6 Each of these results in a new conic formulation of the problem, Cold-start with S/C 7.0 anewdualproblem,andadifferentKKTsystemtosolveineach Warm-start without S/C 4.0 Newton step. The same structure for the KKT coefficient matrix, Warm-start with S/C 3.9 however, will occur in all cases (possibly after elimination of Both short-circuiting and warm-start techniques reduce the total some variables), which means that our method can be used to number of Newton steps required. (Although the combination compute the Newton step efficiently, and therefore, to solve the problem efficiently. of both does not give much further reduction over warm-start sumsq alone.) With warm-start, we require about 4 Newton steps per Let us give some details for the case with objective F . FOP; in our implementation, this corresponds to around 200 µs We can just as well minimize its square root, i.e., solve the per FOP. problem To illustrate dual short-circuiting, we solved grasp contact minimize F point optimization problems. At each iteration, we update the subject to (f,F) K (i) ∈ (34) position and orientation of the manipulator, compute the contact f Ki ,i=1,...,M points, and then, solve the associated FOP. We used a simple Af +∈ωext =0 hill-climbing algorithm to (locally) optimize the manipulator position and orientation. Each of these optimization runs re- with variables F R and f R3M .Here,K denotes the stan- ∈ ∈ quired around 130 or so iterations, each of which requires the dard second-order cone in R3M +1: solution of a FOP. Once again, we considered four schemes: K = (z,y) R3M R z y . cold- and warm-start, with and without dual short-circuiting. { ∈ × |# #≤ } In dual short-circuiting. we stop solving any FOP when it The only difference between this problem and the FOP (8) is that is determined that the set of contact points being considered the cone constraints (f (i) ,F) K, i =1,...,M,thatappearin cannot give a better solution than the best set of contact points the FOP are replaced with the single∈ cone constraint (f,F) K. found so far. The following table shows the average number of Following the methods used in Section III, we arrive at∈ the Newton steps required, per FOP solved, for the three different dual problem methods. maximize νT ωext Method Newton steps per FOP subject to zi Ki∗,i=1,...,M M∈ T 2 A ν zi 1 Cold-start without S/C 10.6 i=1 # i − # ≤ Cold-start with S/C 9.7 the analog of (13). Indeed,/ the only difference between this dual Warm-start without S/C 4.6 and the FOP dual (13) is that the sum of norms constraint in the Warm-start with S/C 3.0 FOP dual becomes a sum of norms squared constraint here. We

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. BOYD AND WEGBREIT: FAST COMPUTATION OF OPTIMAL CONTACT FORCES 1131

4M 6 4M can write a compact and explicit dual, the analog of (14), as With these new definitions of f R , A R × ,and 4 ∈ ∈ T ext K R ,andassumingthatwemeasurethesizeofasetof maximize ν ω i ⊆ M T 2 contact forces by the maximum magnitude of the contact forces subject to dist(A ν,K∗) 1. i=1 i i ≤ (and torque), the FOP is unchanged; it has exactly the form The barrier function for/ the minimum sum-of-squares FOP (34) (7). The dual is also identical, with the dual of the soft contact is given by friction cone 2 2 φ(f,F)= log(F f 2 ) 4 2 2 2 2 2 − −# # K∗ = y R µ (y + y )+σ y y3 . M i ∈ | i 1 2 i 4 ≤ & ( log(µ2 f (i)2 f (i)2 f (i)2). % − i z − x − y i=1 The dual problem is exactly the same; the only change is in , the explicit formula for the Euclidean distance to K . While the The matrix appearing in the KKT system that defines the i∗ Euclidean distance to the aforementioned cone can be computed Newton step, in fact, is fully dense. However, closer examination quickly, there is no longer an explicit formula for it, analogous reveals that the Hessian term contributed by the second term of to (15). the barrier is block diagonal, with 3 3blocks,andtheHessian The barrier method is the same, with the obvious modification term contributed by the first block is× actually diagonal plus rank to the barrier function. The KKT system for the barrier subprob- one. Using an un-elimination step (see [Appendix C, 29]), we lem has the same general form, except that the diagonal blocks then obtain a set of linear equations with exactly the form (22). are 4 4, instead of 3 3. Otherwise, everything is the same. × × B. Contact Force Constraints X. CONCLUSION In this section, we briefly describe, at a high level, how ad- ditional (convex) contact force constraints f can be added We have considered the problem of computing an optimal set to our FOP. The dual objective for the constrained∈C problem will of contact forces, as measured by the maximum magnitude of have an additional term (corresponding to the force constraint the contact forces, subject to the constraint that we hold an object in the primal) that can be used to increase the dual objective. in equilibrium against an external wrench. We have developed In the barrier method, we add a log barrier for the force con- averysimpledualproblem,thatcanbeinterpretedeasily,and straints. This new barrier term contributes a term to the gradient gives a very cheaply computable lower bound on the optimal and Hessian. If the force constraints are separable, i.e., have the grasping force. We have developed a primal barrier algorithm form f (i) (i),thenthecorrespondingHessiantermisblock that reliably solves the FOP within ten or so iterations; in con- diagonal,∈ withC 3 3blocks.Thisimpliesthatthesamebasic trast, general interior-point methods typically take several or method can be used× to efficiently compute the search direction more tens of iterations. Each iteration of our algorithm involves in this case. In particular, the computational effort is still O(M). solving a set of linear equations. The special structure of these equations can be exploited to solve these equations with O(M) 3 C. Other Friction Models computational effort, instead of O(M ) for the naive method. The constant hidden in the O(M) notation is quite modest. For The methods we have described can be extended to other cases when the number of contact forces is small, the advantage friction models, such as frictionless point contact or soft con- here is not huge; but for M large (say, a few tens), the advantage tact with elliptic or linearized approximation (see, e.g., [26], is great. Our nonoptimized implementation can solve a FOP, [32]–[34]). with five contact forces, in around 400 µs. Our method can han- In this section, we briefly explain how to handle one such dle efficient warm-start, exploiting a good initial guess to solve extension to soft contact with elliptic model. In this model, asetofsimilarFOPs.Thisreducestheeffortbyafactorof2. the contact points can exert a torque, about the (local) z-axis (i.e., normal axis), as well as a force. So, we introduce normal REFERENCES contact torques, f (i) R, i =1,...,M,asafourthcomponent t ∈ of the contact point force vector. In this case, the matrices Ai [1] A. Bicchi and V. Kumar, “Robotic grasping and contact: A review,” in become 6 4, with a fourth column that includes the effect of Proc. IEEE Int. Conf. Robot. Autom. (ICRA),2000,pp.348–353. × [2] J. Kerr and B. Roth, “Analysis of multifingered hands,” Int. J. Robot. the torques applied in the normal direction at each contact point; Res.,vol.4,no.4,pp.3–17,1986. the equilibrium condition is still Af + ωext =0,withthesenew [3] Z. Li and S. Sastry, “Task-oriented optimal grasping by multifingered definitions of f and A.Thefrictioncone,nowinR4 ,hasthe robot hands,” IEEE J. Robot. Autom.,vol.4,no.1,pp.32–44,Feb.1988. [4] C. Borst, M. Fischer, and G. Hirzinger, “Grasp planning: How to choose form asuitabletaskwrenchspace,”inProc. IEEE Int. Conf. Robot. Autom. (ICRA),NewOrleans,LA,Apr.2004,pp.319–325. 4 2 2 2 2 2 [5] D. Kirkpatrick, B. Mishra, and C. Yap, “Quantitative Steinitz’s theorems Ki = x R (x + x )/µ + x /σ x3 (35) ∈ 1 2 i 4 i ≤ with applications to multifingered grasping,” in Proc. 20th ACM Symp. & ' ( ' % Theory Comput.,1990,pp.341–351. i =1,...,M,where' σi > 0 is a parameter, sometimes called [6] C. Ferrari and J. Canny, “Planning optimal grasps,” in Proc. 1992 IEEE ' Int. Conf. Robot. Autom.,Nice,France,pp.2290–2295. the torsional friction coefficient [cf. (2)]. Note that here x3 rep- [7] F. Cheng and D. Orin, “Efficient algorithms for optimal force resents the normal force, (x1 ,x2 ) the tangential friction force, distribution—The compact dual LP method,” IEEE Trans. Robot. Au- and x4 the friction torque. tom.,vol.6,no.2,pp.178–187,Apr.1990.

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply. 1132 IEEE TRANSACTIONS ON ROBOTICS, VOL. 23, NO. 6, DECEMBER 2007

[8] Y. Liu and M. Wang, “Qualitative test and force optimization of 3D fric- [31] Yu. Nesterov and A. Nemirovsky, “Interior-point polynomial methods tional force-closure grasps using linear programming,” in Proc. IEEE Int. in convex programming,” in Studies in Applied Mathematics, vol. 13. Conf. Robot. Autom.,May1998,pp.3335–3340. Philadelphia, PA: SIAM, 1994. [9] Y. Liu, “Qualitative test and force optimization of 3-D frictional force- [32] R. Howe and M. Cutkosky, “Practical force-motion models for sliding closure grasps using linear programming,” IEEE Trans. Robot. Autom., manipulation,” Int. J. Robot. Res.,vol.15,no.6,pp.557–572,1996. vol. 15, no. 1, pp. 163–173, Feb. 1999. [33] R. Howe, I. Kao, and M. Cutkosky, “The sliding of robot fingers under [10] B. Mishra, “Grasp metrics: Optimality and complexity” in Algorithmic combined torsion and shear loading,” in Proc. IEEE Int. Conf. Robot. Foundations of Robotics, K. Goldberg, D. Halperin, J.-C. Latombe, and Autom.,1988,pp.103–105. R. Wilson, Eds. Wellesley, MA: A. K. Peters, 1995, pp. 137–166. [34] N. Xydas and I. Kao, “Modeling of contact mechanics and friction limit [11] M. Hanes, S. Ahalt, K. Mirza, and D. Orin, “Neural network control of surfaces for soft fingers in robotics, with experimental results,” Int. J. force distribution for power grasp,” in Proc. IEEE Int. Conf. Robot. Autom. Robot. Res.,vol.18,no.9,pp.941–950,1999. (ICRA),1991,pp.746–751. [12] Y. Xia, J. Wang, and L.-M. Fok, “Grasping-force optimization for mul- tifingered robotic hands using a recurrent neural network,” IEEE Trans. Robot. Autom.,vol.20,no.3,pp.549–554,Jun.2004. [13] M. Teichmann and B. Mishra, “Probabilistic algorithms for efficient grasp- Stephen P. Boyd (S’82–M’85–SM’97–F’99) re- ing and fixturing,” Algorithmica,vol.26,no.3/4,pp.345–363,2000. ceived the A.B. degree (summa cum laude) in math- [14] N. Pollard, “Synthesizing grasps from generalized prototypes,” in Proc. ematics from Harvard University, Cambridge, MA, IEEE Int. Conf. Robot. Autom.,Apr.1996,vol.3,pp.2124–2130. in 1980, and the Ph.D. degree in electrical engi- [15] N. Pollard, “Quality-preserving algorithms for grasp synthesis from pro- neering and computer science from the University of totypes,” IEEE Trans. Robot. Autom.,tobepublished. California at Berkeley, Berkeley, in 1985. [16] G. Farber¨ and R. Suarez,´ “Fast on-line determination of quasi-optimal He is currently the Samsung Professor of engineer- grasp configurations based on off-line analysis and parametrization ing in the Information Systems Laboratory, Depart- of wrist position,” Institut d/Organitzacio i Control de Sistemes Ind.. ment of Electrical Engineering, Stanford University, Barcelona, Spain, Tech. Rep. IOC-DT-P-2005-1, Jan. 2005. Stanford, CA. His current research interests include [17] G. Wohlke,¨ “Grasp planning & force computation for dextrous object convex programming applications in control, signal manipulation with multi-finger robot hands,” Int. Comput. Sci. Inst. processing, and circuit design. He is the author of the books Linear Controller Berkeley, CA, Tech. Rep. TR-94-018, Aug. 1994. Design: Limits of Performance (1991), Linear Matrix Inequalities in System [18] D. Hong Contact force distribution for robotic hand grasping. Blacksburg, and Control Theory (1994), and Convex Optimization (2004). Tech. Rep., 2004. Prof. Boyd is the recipient of the Office of Naval Research (ONR) Young In- [19] M. Nahon and J. Angeles, “Real-time force optimization in parallel kine- vestigator Award, a Presidential Young Investigator Award, the 1992 American matic chains under inequality constraints,” IEEE Trans. Robot. Autom., Automatic Control Council (AACC) Donald P.Eckman Award, the Perrin Award vol. 8, no. 4, pp. 439–450, Aug. 1992. for Outstanding Undergraduate Teaching in the School of Engineering, and an [20] C. Remond, V. Perdereau, and M. Drouin, “A multi-fingered hand con- Associate Students of Standford University (ASSU) Graduate Teaching Award. trol structure with on-line grasping force optimization,” Laboratoire ides In 2003, he received the AACC Ragazzini Education Award. He is a Distin- Instruments et Systemes.` Paris, France. guished Lecturer of the IEEE Control Systems Society, and holds an honorary [21] X. Chen, K. Watanabe, K. Kiguchi, and K. Izumi, “Optimal force distri- Ph.D. from the Royal Institute of Technology (KTH), Stockholm, Sweden. bution for the legs of a quadruped robot,” Mach. Intell. Robot. Control, vol. 1, no. 2, pp. 87–94, 1999. [22] G. Liu and Z. Li, “Real-time grasping-force optimization for multi- fingered manipulation: Theory and experiments,” IEEE/ASME Trans. Mechatron.,vol.9,no.1,pp.65–77,Mar.2004. [23] R. Murray, S. Sastry, and Z. Li, A Mathematical Introduction to Robotic Manipulation. Boca Raton, FL: CRC Press, 1994. Ben Wegbreit (M’71) received the B.E.S. de- [24] M. Buss, H. Hashimoto, and J. Moore, “Dextrous hand grasping force gree in engineering from Johns Hopkins University, optimization,” IEEE Trans. Robot. Autom.,vol.12,no.3,pp.406–418, Baltimore, MD, in 1966, and the Ph.D. degree Jun. 1996. in applied mathematics from Harvard University, [25] M. Buss, L. Faybusovich, and J. Moore, “Dikin-type algorithms for dex- Cambridge, MA, in 1970. terous grasping force optimization,” Int. J. Robot. Res.,vol.17,no.8, He is the Chairman and the CEO of Strider Labs, pp. 831–839, 1998. Palo Alto, CA, a robotics company. His current re- [26] L. Han, J. Trinkle, and Z. Li, “Grasp analysis as linear matrix inequality search interests include robotic manipulation, end ef- problems,” IEEE Trans. Robot. Autom.,vol.16,no.6,pp.663–674,Dec. fector mechanisms, vision, robotic planning, and the 2000. broad commercialization of robotics. He taught at [27] U. Helmke, K. Huper,¨ and J. Moore, “Quadratically convergent algorithms Harvard, and then, joined Xerox PARC, where he for optimal dextrous hand grasping,” IEEE Trans. Robot. Autom.,vol.18, worked in its halcyon years. He has co-founded several companies, two of which no. 2, pp. 138–146, Apr. 2002. he took public: Convergent Technologies and Epiphany. He spent a decade as a [28] M. Lobo, L. Vandenberghe, S. Boyd, and H. Lebret, “Applications venture capitalist. He has served on the board of directors of more than a dozen of second-order cone programming,” Linear Algebra Appl.,vol.284, companies. Additionally, he has made early-stage private investments in various pp. 193–228, Nov. 1998. companies including Genentech, Adobe, Mercury Interactive, and iRobot. He [29] S. Boyd and L. Vandenberghe, Convex Optimization.Cambridge,U.K.: has been a Visiting Scientist at Stanford University, Harvard, and Massachusetts Cambridge Univ. Press, 2004. Institute of Technology (MIT). He has served on the editorial board of the Com- [30] H. Mittelmann, “An independent benchmarking of SDP and SOCP munications of the Association for Computing Machinery (ACM). solvers,” Math. Program. Ser. B,vol.95,pp.407–430,2003. Dr. Wegbreit elected as a Fellow of the ACM, in 1993.

Authorized licensed use limited to: Stanford University. Downloaded on April 30,2010 at 21:16:57 UTC from IEEE Xplore. Restrictions apply.