Fast Grasp Planning for Hand/Arm SystemsBased on Convex Model

Kensuke Harada, Kenji Kaneko, and Fumio Kanehiro

Abstract— This paper discusses the grasp planning of amultifingered hand attached at the tip of a robotic arm. By usingthe convex models and the new approximation method of thefriction cone, our proposed algorithm can calculate the graspingmotion within the reasonable time. For each grasping styleused in this research, we define the grasping rectangular convex(GRC). We also define the object convex polygon (OCP) for thegrasped object. By considering the geometrical relashionshipamong these convex models, we determine several parametersneeded to define the final grasping configuration. To determinethe contact point position satisfying the force closure, weuse two approximation models of the friction cone. To savethe calculation time, the rough approximation by using theellipsoid is mainly used to check the force closure. Additionally,approximation by using the convex polyhedral cone is used atthe final stage of the planning. The effectiveness of the proposedmethod is confirmed by some numerical examples.

I. INTRODUCTION

A multi-fingered hand has the potential possibility to graspseveral objects with different shape. However, the graspplanning of a multi-fingered hand often becomes difficult dueto its complexity. In many cases, since a hand/arm system hasmore than 20 dof, we have to plan the grasping motion in thehigh-dimensional configuration space by using the followingstrategy; First, although there are plenty of possible graspingstyles[18], we have to select a feasible one according to therequired task. Then, we have to determine the contact pointposition on both the fingers and the grasped object. Thecontact point position has to be determined so that the fingercan grasp the object without dropping it within the limit ofthe actuator power. Finally, we have to plan the motion ofthe hand/arm system from the initial configuration to the finalone with avoiding unnecessary collision among the fingersand the environment.

Now, let us consider some typical cases where the graspplanning is needed. The first example is the humanoid robothaving a multi-fingered hand as shown in Fig.1. When ahumanoid robot grasps an object, the humanoid robot willfirst measures the position/orientation of the object by usingthe vision sensor attached at the head. Then, based onthis information, the grasp planner plans the motion of thehand/arm system to realize the final grasping posture. Oncethe planning finishes, the multi-fingered hand will pick upthe object by grasping it. We can also consider the pick-and-place task in a manufacturing factory. In this case,several objects randomly placed on the belt conveyer will be

The authors are with Humanoid Research Group, Intelligent SystemsResearch Institute, National Institute of Advanced Industrial Science andTechnology (AIST), Japan kensuke.harada@aist.go.jp

Fig. 1. Humanoid HRP3P with multi-fingered hand

delivered to the industrial robot manipulator. By measuringthe position/orientation of the objects, the robot will plan thepick-and-place motion of the object. For both of the abovecases, the grasping motion has to be calculated as fast aspossible. Thus, as for the grasp planning, a heuristic but fastalgorithm will be preferred rather than a precise but time-consuming algorithm[2].

In this paper, we propose a fast grasp planning algorithmfor hand/arm systems. Our proposed algorithm can calculatethe grasping motion within the reasonable time simultane-ously satisfying several conditions for the grasp system suchas the feasible grasping style, the friction cone constraint,the maximum contact force, the inverse kinematics of thearm and the fingers, and the collision among the fingers etc.The contribution of this paper is as follows; Our proposedalgorithm first assumes the convex models for both theobject and the grasping region of the hand. By consideringthe geometrical relashionship between two models, we candetermine several parameters needed to plan the grasp con-figuration. This is just like the problem of checking whethera block can be put into a box or not. Then, we determine thecontact point position between the fingertip and the object.These points are determined so that the force closure can besatisfied. However, so as to reduce the calculation time, wepropose two methods for testing the force closure. The firstmethod is mainly used where it approximates the frictioncone by using ellipsoid. This is a rough approximation butcan quickly estimate the force closure. Additionally, we usemore precise approximation by using the convex polyhedralcone. Furthermore, to determine the contact point position,we use the random sampling approach. The random samplingcontributes to save the calculation time since we do notneed to analytically obtain the region of the contact point

2008 IEEE International Conference onRobotics and AutomationPasadena, CA, USA, May 19-23, 2008

978-1-4244-1647-9/08/$25.00 ©2008 IEEE. 1162

position satisfying the force closure. Finally, the effectivenessof the proposed algorithm is confirmed by some numericalexamples.

II. RELATED WORKS

As for the works on grasp planning, there are a number ofworks on contact-level grasp synthesis such as [10], [11], [9].As for the grasp planning considering the hand/arm model,Cutkosky[1] first proposed an expert system to select oneof the grasp styles based on the grasp quality measures.Pollard[8] proposed a precise grasp planning algorithm byutilizing heavy computation. Morales et al.[7] proposed aplanning method applicable to the Barret hand. Moraleset al.[16] also proposed a method of planning a grasp bypreparing the candidate grasping style corresponding to thegrasped object. Prats et al.[17] proposed a method of forplanning the hooking task of a door.

Recently, some researchers researched the fast grasp plan-ning problem. Borst et al.[13] proposed a planning methodto determine the contact point position of a four-fingeredhand assuming the fingertip grasp. Miller et al.[4] uses thegrasping simulator[5] and proposed a method for determin-ing the candidates of the contact point set for the graspplanning. Their method calculates 1 ∼ 44 candidates ofgrasp configuration between 11.4[s] and 478[s] dependingon the complexity of the object’s shape. Their approach wasextended to the learning approach [6] and imperfect visualinformation [3].

On the other hand, we propose a planning method forgeneral multi-fingered hand/arm systems taking all functionsexplained in the introduction into account. Our method iseasy and fast because the grasping style and the nominalposture are determined by using the relationship betweentwo convex models and because the approximation of thefriction cone by using the ellipsoid is used.

As for the grasp planning based on the random sam-pling approach, Niparnan[9] and Borst et al.[12] proposeda method to realize the force closure using the random sam-pling. Yashima et al.[15] proposed a manipulation planningbased on the random sampling.

III. GRASPING STYLE SELECTION

A. Reference Posture and Rectangular Convex

When a human grasps an object, a human selects one ofthe grasp styles according to the shape and the expected massof the object. Fig.2 shows some of the grasp styles shownin the book [18]. The prehension by subterminal opposition(213) and the prehension by subtermino-lateral opposition(214) are used when grasping a thin and light object. Thepollici-tridigital (221) is used when grasping a pencil likeobject. On the other hand, as for the digito-palmar prehension(227) and the full palmar prehension (229), the hand cancontact the object at the inner-link of a finger and at thepalm. These grasp styles are used when grasping a large andheavy object.

We assume that the shape of the object is given by apolygon model such as VRML. For given shape of the object,

Fig. 2. Human grasping styles

Fig. 3. Reference posture and grasping rectangular convex (GRC)

the grasp planner has to automatically select one feasiblegrasp style. To realize this function, we assume the referenceposture of the hand and the grasping rectangular convex(GRC) for each grasping style as shown in Fig.3. Here, theGRC expresses the maximum size of the object which thehand can grasp. The GRC is used to roughly predict whetherthe hand can grasp the object or not.

In this research, we manually constructed both the refer-ence posture and the GRC. Automatic generation of themis our future research topic. For example, we can constructboth the reference posture and the GRC by considering theforce closure condition, the manipulability of the fingers etc.Also, we can generate the reference posture by capturing the

1163

Fig. 4. Object and object convex polygon (OCP)

human motion[19].By modifying the reference posture, the actual grasping

posture is planned by using the method explained in thenext section. For the i-th grasping style with the GRC whichposition/orientation is ppri/

pRri (i = 1, · · · , n), we assumethe vector of edge length peri of the GRC. This vector is usedto select the grasping style. Also, we assume the approachvector pdri. This vector defines the approach direction tothe object and is one of the outer unit normal vector of theGRC’s surface. Furthermore, we assume the maximum massmmax,i of the object grasped by using the i-th grasping style.

Next, we focus on the object to be grasped. Given theshape of the object, we calculate the object convex polygon(OCP). The OCP is the minimum sized convex polygonincluding the grasped object. In this paper, we consider thecase where the rectangular convex is used as the OCP. Forcomplex shaped objects, we consider splitting the object intosome regions and calculate the OCP for each region. Fig.4shows the OCP of the vase placed on the table. We splitthe object into three regions and obtained the OCP for eachregion.

Once the polygon model of the grasped object is given, wehave the set of points included in the surface of the object.By using the eigen vectors of the co-variance matrix of thepoint set, we can calculate the OCP. For the i-th OCP whichposition/orientation is poi/Roi (i = 1, · · · , m), we assumethe vector of edge length eoi.

Miller et al.[4], [6] also used the convex model for thegrasped object. In this research, in addition to the OCP,we use the GRC and determine the grasping style and thenominal grasping posture.

B. Nominal Position/Orientation of Palm

Let us consider selecting one of the grasping styles anddetermining the nominal position/orientation of the palm.For such purpose, we introduce some heuristic rules in thissubsection. We first focus on the geometrical relationshipbetween the GRC and the OCP. Let sort(a) be the functionreplacing the elements of the vector a in a decreasing order.We consider the following function.

bij = sort(eri) − sort(eoj) i = 1, · · · , n, j = 1, · · · , m(1)

Fig. 5. Four possibilities for palm position/orientation

If bi > 0 is satisfied, the OCP can be included inside theGRC. For the enveloping grasp as shown in Fig. 3 (c),(d) and (e) satisfying bi > 0, there is a possibility thatthe hand can grasp the object by using the i-th graspingstyle. On the other hand, for the fingertip grasp as shown inFig. 3, eq.(1) was checked only for two edges of the GRCwhich surface defines the approach vector. We also assumethe maximum weight of the object for each GRC. We donot select the grasping style where the object’s weight islarger than the maximum weight of the GRC. If there aremultiple candidates of grasping styles, we consider selectingone grasping style where the amount of modification fromthe reference posture is expected to the smallest.

Once the grasping style is determined, we next determinethe nominal position/orientation of the palm. Fig.5 shows theoverview of the method. Let us focus on the surface of theGRC having the approach vector dri as the normal. We makethis surface contact the surface of the OCP. Assuming thatboth rectangular convex contact each other at the center ofthe rectangle, there are four possibilities for the orientation ofthe GRC as shown in (b),(c),(d) and (e). For this example, theorientation of the GRC shown in (b) and (d) is not feasiblesince the GRC does not include the OCP even if the GRCmoves to the direction of the approach vector. On the otherhand, for the position/orientation of the palm shown in (a)and (c), we try to solve the inverse kinematics of the arm.If the inverse kinematics problem has a solution, we havea candidate of the nominal position/orientation of the palm.We iterate this calculation for all the surface of the OCPwithout contacting another OCP.

In case where we have multiple candidates of the nominalposition/orientation of the palm, we have to choose one. Inthis research, we applied the nominal position/orientation ofthe palm where the norm of joint angle of the wrist becomesminimum.

IV. FINAL GRASPING POSTURE

By using the nominal position/orientation of the palm,now we determine the final grasping posture. The graspingposture is determined so as to satisfy several constraintsimposed on the grasp system.

1164

Fig. 6. Two approximation methods for friction cone constraint

A. Force Closure Condition

The grasped object has to resist the external wrenchwithout breaking the contact. For this purpose, we formulatethe wrench set generated at a point in the object. To formulatethe wrench set, we usually approximate the friction coneconstraint at each contact point since the friction cone con-straint is nonlinear and is difficult to deal with. As shown inFig.6, we introduce two approximation methods, i.e., (a) theintersection of an ellipsoid and a sphere and (b) the convexpolyhedral cone. (b) is a conventional approximating methodwhile (a) is a newly proposed one. Although (a) is a roughapproximation, we can roughly check if the applied wrenchis included in its set by just calculating a few equations.In our planning algorithm, (a) is used to plan the graspingposture while (b) is additionally used to confirm the forceclosure condition at the final stage of the planning algorithm.

1) Approximation by intersection of ellipsoid and sphere:In this method, we consider approximating the friction coneconstraint by using the ellipsoid and approximating the max-imum normal component of the contact force by using thesphere (Fig.6 (b)). In the following, although we formulatejust for the ellipsoid approximating the friction cone, we canformulate the sphere with the same fashion.

Let us consider the contact force f i (i = 1, · · · , n)applied at the i-th contact point position pi. The ellipsoidapproximating the friction cone can be expressed as

(f i − fmaxni)tU iSU ti(f i − fmaxni) ≤ 1 (2)

where S = diag[μfmax μfmax αfmax] and U i is a 3 × 3matrix composed of the unit normal and tangent vectors.This is a rough approximation since this ellipsoid doesnot include very small contact forces. Also, it may includethe contact force outside of the actual friction cone if thenormal component of the contact force is large. By usingthis equation for n contact points, the set of the wrench wgenerated by the object can be given by

f(p1, · · · , pn) =(w − fmaxGN)t(GU−1Gt)−1(w − fmaxGN) ≤ n

(3)

where

G =[

I · · · Iγp1× · · · γpn×

],

N = [nt1 · · · nt

n]t,U = block diag[U1SU t

1 · · · UnSU tn].

When we apply eq.(3) to the planner, we impose a marginδf from the boundary of the wrench set and check n −f(p1, · · · , p) ≥ δf . This method is useful since we canroughly check the force closure condition just by calculatingthe left-hand side of eq.(3).

2) Approximation by convex polyhedral cone: The ap-proximation by the convex polyhedral cone can be a preciseone if we increase the number of the span vectors. However,calculation of the wrench set becomes relatively heavy. Inthis section, we introduce an easy and simple method ofjudging the force closure by using the software package“qhull”[23]. By using the qhull, we can calculate the convexhull and the Delaunay triangulation of a point set.

Considering the combination of the contact force at eachcontact point lying along one of the span vectors, we cancalculate the wrench set wi, i = 1, · · · , m applied at a pointin the object. Then, by using the qhull, we can calculate theconvex hull of the wrench set as follows:

W = ConvexHull {w1, · · · , wm} . (4)

We can easily judge the force closure by adding the gravitywrench wg of the object to the wrench set. Let us considerthe following convex hull:

W̃ = ConvexHull {wg, w1, · · · , wm} . (5)

If the vertices of W̃ do not include wg and the distancebetween wg and each surface of W̃ is not zero, the graspsystem is found out to be the force closure.

B. Random Sampling

To obtain the final grasping posture, we use the randomsampling technique. Let n be the number of fingers. Weuse 1 + n variables to search for the final grasping posture;Δpp ∈ R3, and Δai ∈ R3 (i = 1, · · · , n).

As shown in Fig.7, the three dimensional vector Δpp

expresses the position of the OCP w.r.t. its nominal positionand is used to determine the position of the palm. On theother hand, Δpi (i = 1, · · · , n) expresses the position of thefingertip w.r.t. its nominal position.

1165

Fig. 7. Sample the position of OCP w.r.t. GRC

C. Planning Algorithm

We assume that we have the polygon model of boththe finger and the grasped object. To check the collisionbetween the finger and the object, we used the softwarePQP (Proximity Query Package). By using PQP, even if twopolygon models do not contact each other, we can calculatethe distance between two models, the point on both modelswhere the distance becomes minimum, and the unit normalvector of the model’s surface.

We first explain the method to find the posture of a fingercontacting the object. Although, roughly speaking, there aretwo methods where one is to solve the inverse kinematics sothat the finger link contacts the object at the desired positionand the other is to directly compensate the joint angles, weused the latter approach in this research. First, correspondingto each grasping style, we defined the links of the fingers tocontact the object. Then, for each defined link, we assign ajoint of a finger to compensate its position. We changed theangle of the assigned joint by a small amount and checkedthe collision between the link and the object. We iterate thiscalculation until the distance between the link and the objectis smaller than a predefined value.

By using the pseudocode shown in Algorithm 1, wesummarize the algorithm explained in this section. In thisalgorithm, after confirming that n − f(p1, · · · , pn) ≥ δf issatisfied, then we calculate the force closure using the qhull.

Algorithm 1 Determination of Grasping Posture for n finger grasploop

loopSample Δpp.if Arm IK is solvable then break

end loopfor i = 1 to n

for j = 1 to mSample Δpi.if Finger i IK is solvable then

if Found finger i posture contacting object then breakend if

end forif Not found finger i posture then break

end forif n − f(p1, · · · , pn) ≥ δf then

if Force closure using qhull then breakend if

end loop

Fig. 8. Path planning using the convex hull model

V. PATH PLANNING

After the final posture of the hand is determined, we planthe trajectory between the initial and the final postures. Asshown in Fig. 8, we set the intermediate goal of the palm’sposition/orientation where the surface of the GRC contactsthe surface of the OCP. We first plan the position/orientationof the palm finally reaching the intermediate goal. At theintermediate goal, the fingers are fully opened. Then, thepalm is translated to the final position. After the palm reachedthe final position, the fingers begin to close until the finalgrasping posture is realized. In this planning algorithm, wedid not assume that the obstacle exists between the initialposture and the intermediate goal. If the obstacle exists, weuse the probabilistic roadmap (PRM) to search for the path[22], [24].

VI. NUMERICAL EXAMPLE

To confirm the effectiveness of the proposed method, weshow some numerical examples. As a model of the hand/armsystem, we use the 7dof arm of the HRP-3P[21] and adeveloped 4 fingered hand [20]. This 4 fingered hand hasthe thumb with 5 joints and the index, middle and thirdfingers with 4 joints. The most distal joint of each fingeris not directly actuated and moves along with the next joint.

We prepared 6 reference motion as shown in Table I. Weused a vase with 0.5[kg] as a grasped object. Fig.9 showsthe overview of the simulation environment. In this numericalexample, we make only the most distal link of each fingercontacts the object, for simplicity. As shown in Fig.4, wemanually split the vase into three regions and calculated theOCP. As for B shown in Fig.4, the edge length of the OCPwas calculated as (0.22, 0.067, 0.067) [m].

The 229 style is selected by using the algorithm shownin Section 3. Fig.10(a),(b) and (c) shows the motion of thefingers. Also, Fig.10(d) shows the 3 dimensional subset ofthe wrench set obtained by the qhull. We performed thisnumerical example for 5 times. And, it takes between 10 [s]and 25 [sec] to calculate the final grasping posture using the2[GHz]. However, almost 80% of the calculation time wasspent to the calculation of qhull although qhull was calledjust once for 3 of 5 trials.

Fig.11 shows the grasp of a cellular phone and a shavingmachine. The 221 style was used for the cellular phone whilethe 229 style was used for the shaving machine.

1166

TABLE IEDGE LENGTH OF EACH GRC

Ref. no. eri [m]213 (0.1, 0.06, 0.03)214 (0.015, 0.06, 0.08)217 (0.015, 0.1, 0.015)221 (0.3, 0.06, 0.03)228 (0.3, 0.03, 0.1)229 (0.5, 0.07, 0.07)

Fig. 9. Simulation environment

Fig. 12 shows the simulation result of using the PRMplanner to avoid the collision between the arm/hand systemand the table. In this case, we plan the motion in the 24dimensional configuration space. It takes about 5 [s] tocalculate the path between the start and the goal. As you cansee from this figure, the arm avoids the collision between thetable edge and the arm.

VII. CONCLUSIONS AND FUTURE WORKS

In this paper, we proposed an algorithm of grasp planningfor hand/arm systems. By using the convex model for bothhand and the object, we showed that the nominal palmposition/orientation can be calculated easily. Also by usingthe ellipsoidal approximation of the friction cone, we showedthat the final grasping posture can be calculated within ashort period of time.

This paper is the first step of our research and we havea plenty of future research topics. First, we have to furthermake the calculation time shorter. Instead of using the qhull,we plan to use the force closure test algorithm proposed in[13]. Also, we plan to consider another approximation modelof the friction cone by using multiple ellipsoids. Second,we will apply the proposed algorithm for several shape ofobjects and confirm the feasibility of the algorithm. Also,we plan to verify the effectiveness of the proposed methodby experiments. Finally, we plan to construct the sensorfeedback algorithm since our finger has 6 dof force/torquesensors at the tip.

REFERENCES

[1] M.R. Cutkosky, On Grasp Choice, Grasp Models, and the Designof Hands for Manufacturing Tasks, IEEE Trans. on Robitics andAutomation, vol.5, no.3, 1989.

[2] K.B. Shimoga, Robot Grasp Synthesis: A Survey. Int. J. of RoboticsResearch, vol.5, no.3, 1996.

Fig. 10. Result of the proposed method

[3] S. Ekvall and D. Kragic, Learning and Evaluation of the ApproachVector for Automatic Grasp Generation and Planning, IEEE Int. Confon Robotics and Automation, 2007.

[4] A.T. Miller, S. Knoop, H.I. Christensen, and P.K. Allen, AutomaticGrasp Planning using Shape Primitives, IEEE Int. Conf. on Roboticsand Automation, 2003.

[5] A.T. Miller and Peter K. Allen, GraspIt!: A Versatile Simulator forGrasp Analysis, ASME Int. Mechanical Engineering Congress &Exposition, 2000.

[6] R. Pelossof, A. Miller, P. Allen, and T. Jebra, An SVM LearningApproach to Robotic Grasping, IEEE Int. Conf. on Robotics andAutomation, 2004.

[7] A. Morales, P.J. Sanz, A.P. del Pobil, and A.H. Fagg, Vision-based Three-finger Grasp Synthesis Constrained by Hand Geometry,Robotics and Automous Systems, no.54, 2006.

[8] N.S. Pollard, Closure and Quality Equivalence for Efficient Synthesisof Grasps from Examples, Int. J. of Robotics Research, vol.23, no.6,pp.595-613, 2004.

[9] N. Niparnan and A. Sudsang, Fast Computation of 4-Fingered Force-Closure Grasps from Surface Points, IEEE/RSJ Int. Conf. IntelligentRobots and Systems, 2004.

[10] J.A. Coelho Jr. and R.A. Grupen, Online Grasp Synthesis, IEEE Int.Conf. on Robotics and Automation, 1996.

[11] J. Ponce, S. Sullivan, J.-D. Boissonnat, and J.-P. Merlet, On Character-izing and Computing Three- and Four-fingered Force Closure Graspsof Polygonal Objects, IEEE Int. Conf. on Robotics and Automation,1993.

[12] C. Borst, M. Fischer, G. Hirzinger, Grasping the Dice by Dicing theGrasp, IEEE/RSJ Int. Conf. on Intelligenr Robots and Systems, 2003.

[13] C. Borst, M. Fischer, G. Hirzinger, A fast and robust grasp plannerfor arbitrary 3D objects, IEEE Int. Conf. on Robotics and Automation,1999.

[14] S.B. Kang and K. Ikeuchi, Toward Automatic Robot Instruction fromPerception-temporalsegmentation of Tasks from Human Hand Motion,IEEE Trans. on Robotics and Automation, vol. 11, no. 5, 1995.

[15] M. Yashima, Y. Shiina and H. Yamaguchi, Randomized ManipulationPlanning for A Multi-Fingered Hand by Switching Contact Modes,IEEE Int. Conf. on Robotics and Automation, 2003.

[16] A. Morales, T. Asfour, and P. Azad, Integrated Grasp Planning andVisual Object Localization for a Humanoid Robot with Five-FingeredHands, IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2006.

[17] M. Prats, P.J. Sanz, and P. del Pobil, Task-Oriented Grasping usingHand Preshapes and Task Frames, IEEE Int. Conf. on Robotics andAutomation, 2007.

1167

Fig. 11. Grasp of a cellular phone and a shaving machine

Fig. 12. Result of the PRM planner

[18] I.A. Kapandji, Physiologie Architecture, Maloine S.A. Editeur, 1985.[19] S. Nakaoka, A. Nakazawa, K. Yokoi, H. Hirukawa, and K. Ikeuchi:

Generating Whole Body Motions for a Biped Humanoid Robot fromCaptured Human Dances, IEEE Int. Conf. on Robotics and Automa-tion, 2003.

[20] K.Kaneko, K. Harada, and F. Kanehiro, Development of Multi-fingered Hand for Life-size Humanoid Robots, IEEE Int. Conf. onRobotics and Automation, pp. 913-920, 2007.

[21] K. Akachi, K. Kaneko, N. kanehira, S. Ota, G. Miyamori, M. Hirata,S. Kajita, and F. Kanehiro, Development of Humanoid Robot HRP-3P,IEEE-RAS/RSJ Int. Conference on Humanoid Robots, 2005.

[22] G. Sanchez and J.C. Latombe, A Single-Query Bi-Directional Prob-abilistic Roadmap Planner with Lazy Collision Checking, Int. Symp.on Robotics Research, 2001.

[23] http://www.qhull.org/[24] http://robotics.stanford.edu/˜mitul/mpk/

1168