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 .