Grasp Planning for Customized Grippers by Iterative Surface Fitting
Abstract
Customized grippers have broad applications in industrial assembly lines. Compared with general parallel grippers, the customized grippers have specifically designed fingers to increase the contact area with the workpieces and improve the grasp robustness. However, grasp planning for customized grippers is challenging due to the object variations, surface contacts and structural constraints of the grippers. In this paper, an iterative surface fitting (ISF) algorithm is proposed to plan grasps for customized grippers. ISF simultaneously searches for optimal gripper transformation and finger displacement by minimizing the surface fitting error. A guided sampling is introduced to avoid ISF getting stuck in local optima and improve the collision avoidance performance. The proposed algorithm is able to consider the structural constraints of the gripper, such as the range of jaw width, and plan optimal grasps in realtime. The effectiveness of the algorithm is verified by both simulations and experiments.
I Introduction
Grasping is an essential capability for robots to extend the functionality and execute complex tasks such as assembly, picking and packaging. Compared with general parallel grippers, the customized grippers are designed for particular class of manipulation tasks by matching the contact surface of the gripper with the geometry of the workpieces. As a result, the customized grippers generally have larger gripping force and more robust grasp. For example, the food industry may design particular grippers to grasp cakes from conveyors for food packing, and the autonomous assembly line may require customized grippers to assure stable grasp and precise localization.
However, the grasp planning for customized grippers are challenging. The traditional point contact model or soft finger model [1] in grasp modeling is insufficient to describe the large surface contact between the gripper and the object. Thus, the quality evaluation is ambiguous for one particular grasp configuration. Therefore, it is desirable to exploit the surface information of the gripper and the object for grasp planning.
There are several related works proposed to reveal the importance of contact surfaces in grasp planning. A taxonomy of grasp is constructed in [2, 3] to analyze grasp models and design grippers. The taxonomy observes that both power grasps and precision grasps tend to increase the contact surface to improve the stability and robustness of the grasp, where the object is either surrounded by the palm and fingers in power grasps, or by the soft fingertips in precision grasps.
The fingertip space is proposed in [4] to take into account the matching of the basic fingertip geometry to object surface. The grasps are searched based on several layers of fingertip space with different resolutions. This method assumes point contact between the finger and the object, which is not the case for customized grippers with large contact surfaces. To take advantage of the large contact surface on grasp stability and robustness, [5] searches optimal grasps by minimizing the distance between predefined points on the hand and the surface of the object with the simulated annealing method. The resultant grasps match the object surface by enclosing the object with more contacts. However, the computation load is excessively heavy for online implementations. The grasp synthesis for human hands using shape matching algorithm is proposed in [6]. The algorithm matches hand shapes in a database to the query object by identifying collections of features with similar relative placements and surface normals. However, this approach requires lots of human demonstration to collect enough hand pose samples. Hence the searching for proper grasps requires considerable time with exhaustive sampling. The idea of matching the shapes of parallel grippers to the geometries of objects are utilized in [7, 8] to accelerate the grasp searching speed by filling the parallel gripper with the object; however, this method cannot be generalized to a gripper with complicated shape like customized grippers.
In order to consider more complicated gripper shape and retrieve reliable and secure grasps, it is desired to match the surfaces on the gripper to the object more precisely. The idea of shape registration using iterative closest point (ICP) is first proposed in [9] and then refined by [10, 11, 12]. However, these approaches only consider to align one source object to one target object, while the surface matching in grasp planning for customized grippers can be regarded as registering multiple surfaces on different fingers to one target object. Moreover, the surfaces have to satisfy the constraints of the grippers such as the jaw width, allowable degrees of freedom (DOF) and the alignment of contact normals.
Considering the specific surface matching problem in grasp planning, this paper proposes an iterative surface fitting (ISF) algorithm, where it fits the contact surface based on a metric measuring the distance of gripperobject surface as well as the misalignment of the contact normals. ISF takes the motions of both the palm and fingers into consideration by a proposed iterative palmfinger optimization (IPFO). A guided sampling is also introduced to encourage the exploration of the regions with high fitting scores.
The contributions of this paper are as follows. First, the proposed algorithm includes both the desired grasp surfaces and the structural constraints of the gripper such as the jaw width, the DOF. It also achieves simultaneous surface fitting and gripper kinematic planning. Second, the proposed guided sampling avoids getting stuck in the local optima by exploiting the previous grasping experience. The grasp planning by ISF and guided sampling achieves a realtime planning and the time to search for a collisionfree grasp is less than 0.1 s for the objects in the simulation and experiment. Furthermore, by combining with the dedicated gripper design, the proposed surface fitting algorithm can deal with objects with complicated shapes as well as a clutter task with unsegmented point clouds.
The remainder of this paper is described as follows. The grasp planning problem for customized gripper is stated in Section II, followed by a detailed introduction of the proposed method including the iterative surface fitting and the guided sampling in Section III. Simulation and experiment results are presented in Section IV. Section V concludes the paper and proposes future works.
Ii Problem Statement
A grasp planning example with a customized gripper is shown in Fig. 1. The customized gripper has parallel jaws with curved fingertip surfaces. The object to grasp is a cylinder. The grasp planning problem is to search for the optimal pose of the gripper relative to the object by maximizing a quality metric considering the constraints such as the width of the gripper jaw. More concretely, it is formulated as
(1a)  
(1b)  
(1c)  
(1d)  
(1e) 
where is the finger index, are the rotation and the translation of the gripper jaw from the original pose, is the finger displacement from the original width , and represents the grasp quality with respect to the finger contact surfaces and the object contact surface . The finger contact surface lies on the finger surface transformed by , as shown in (1b). The object contact surface is determined by the nearest neighbor of the on the object surface , as shown in (1c).
Constraint (1d) indicates that the finger contact surfaces should be constrained in the space parameterized by the jaw width, and (1e) describes the constraint of the finger displacement. The optimization (1) searches for the optimal gripper transformation and finger displacement by maximizing the grasp quality .
Problem (1) is a standard grasp planning problem if the contact surfaces are degenerated into points. In general surface contact situation, the point contact model may not be able to incorporate the gripper surface directly into the planning. Problem (1) becomes challenging to solve by either gradient based methods or sampling based methods. On one hand, modeling the contact surfaces as decision variables is nontrivial in the gradient based methods. On the other hand, the sampling based methods require exploring the whole state space, which is not suitable for realtime implementation.
A natural surfacerelated quality can be constructed by matching the surfaces between the object and the gripper. Intuitively, the grasp with small surface matching error (Fig. 1(a)) is more stable and robust compared with the one with large surface matching error (Fig. 1(b)). Therefore, an iterative surface fitting (ISF) algorithm is proposed in this paper to search for optimal grasps of the gripper relative to the object. The optimality is in the sense of surface fitting errors. The formulation is modified from the iterative closest point (ICP) by including the structural constraints of different fingers such as the width and DOF. ISF is initialized by a guided sampling algorithm in order to avoid trapped in a local optima and achieve collision avoidance. These methods are introduced below.
Iii Proposed Methods
In this section, the related ICP is first reviewed in Section IIIA, followed by the introduction of ISF with a one DOF customized gripper in Section IIIB. The planning for multifingered customized gripper is discussed in Section IIIC. Lastly, the guided sampling of ISF is presented in Section IIID.
Iiia Iterative Closest Point
The ICP algorithm is first proposed in [9] for 3D shape registration. It searches for the optimal rigid transformation to align the source surface towards the target surface by minimizing the distance between them. The minimization is conducted by iteratively searching the correspondence points of the surfaces and minimizing the Euclidean distance of these correspondence points. More specifically, it is written as
(2) 
where denote a correspondence pair on the source surface and the target surface, and is the number of the correspondence pairs. The correspondence of are searched by the nearest neighbor method. The optimal transformation is calculated and applied to the source surface, after which the correspondence is updated by searching with the nearest neighbor again. These two steps are performed iteratively until convergence.
A relevant variation proposed by [13] calculates the distance along the normal direction:
(3) 
where indicates the normal of the target surface on point . Instead of calculating the Euclidean distance between the transformed source point and the corresponding target point, the modification calculates the distance of the point to the plane by projecting the vector to normal direction . Therefore, this modification allows the source surface to slide along the flat target surface.
Assume the rotation angle is small, the rotation matrix can be approximated by , where represents the skewsymmetric matrix of the axisangle vector . With this approximation, (3) can be solved analytically by a standard least squares:
(4) 
where , and with
(5a)  
(5b) 
The optimal solution for the transformation is then given by under the current correspondence.
IiiB Iterative Surface Fitting with one DOF Gripper
ICP algorithm, however, can only register one single surface to the target surface at a time. In the grasping applications, a gripper usually consists of two or more fingers, which are all expected to be fitted on the target workpiece. We propose an iterative surface fitting (ISF) algorithm here to register multiple finger surfaces to the target workpiece at the same time considering the motions of fingers.
For ease of illustration, the ISF algorithm is introduced using a customized gripper with two fingers and one DOF, where the two fingers move in opposite directions to adjust the jaw width (Fig. 2). The ISF algorithm iteratively executes two modules: the correspondence matching and the surface fitting. The correspondence matching contains the nearest neighbor search and outlier/duplicate filtering. As shown in Fig. 2(a), given the point on the gripper surface (black dots) on the th finger, the KDTree is employed to search the nearest neighbor on the object surface (red dots), where the dash lines represent their correspondence. The outlier filtering removes the pairs whose distances are excessively large and the duplicate filtering considers the case where multiple points are assigned to the same point on the object. In this case, Only the pair with the minimum distance will be kept, while other correspondences will be removed [12]. The outlier/duplicate filtering are particularly useful in our implementation since the gripper surface and object surface overlap only partially, and rejecting these point pairs greatly increases the robustness.
In Fig. 2(b), the surface fitting aims to search over the gripper transformation as well as the relative motion between the fingers, where and indicate the jaw width before and during the optimization. To be more specific, the surface points are transformed to new locations by
(6) 
where is a unit vector pointing from to , as shown by the red arrow in Fig. 2(b). Then the matching of contact points can be quantified by an error metric, which is given by
(7) 
An additional cost term is constructed as
(8) 
where represents the misalignment error between the normal of the gripper and that of the object . By minimizing , the normals of the finger surface are forced to align towards the normals of the object surface.
The surface fitting is to minimize the surface fitting error
(9a)  
(9b) 
with the overall error and is the weighting factor that balances the the point matching and the normal alignment.
Problem (9) is nonlinear due to the coupling between the and . In this paper, the palm transformation and the finger displacement are solved by an iterative palmfinger optimization (IPFO). The palm optimization optimizes for the optimal palm transformation with fixed , while the finger optimization optimizes for the optimal finger displacement with fixed ().
The palm optimization can be formulated as a least squares problem that is similar to (4) with an augmented matrix and an augmented vector , where and are the point matching of each finger surface modified from (4), with in (5) replaced by for to consider the displacement of the finger. and are the additional term to align the contact normals. Derived from (8), we can get and with and . Hence, the palm optimization has the closed form and can be represented as
(10) 
The finger optimization with fixed in (9) is a onedimensional constrained quadratic programming,
(11a)  
(11b) 
where , and . The optimal finger relative motion is given by
(12) 
with
(13) 
The procedure of IPFO is summarized in Alg. 1.
Given the correspondence and the corresponding normals as inputs, IPFO minimizes the error metric (9) in an iterative manner ( Line 56). The iteration stops when the error reduction is less than a threshold (Line 3).
The ISF algorithm iteratively searches the correspondence and optimizes over , and , as shown in Fig. 2. Inspired by [10], ISF is optimized hierarchically by searching with a multiresolution pyramid, as shown in Alg. 2. The inputs to ISF include the initial gripper state , the surfaces and , and the parameters for hierarchical searching (Line 1). and denote the level number, maximum iteration and error bound for convergence, respectively. The gripper surface is first transformed to the specified initial state (Line 2). In each level of pyramid, the is downsampled adaptively to with different resolutions (Line 4). The while loop iteratively searches correspondence and solves for desired gripper motion. The correspondence matching is conducted with nearest neighbor search (Line 8) and with the outlier/duplicate filtering (Line 9). The desired palm transformation and finger displacement are optimized by IPFO (Line 10). Both the surface and its sample are transformed by the optimized gripper motion (Line 1112). The while loop is terminated if IPFO gives a similar transformation in adjacent iterations.
IiiC Iterative Surface Fitting (ISF) in General Case
The algorithm is able to extend to multifingered grippers. Similar as above, we deal with the coupling by iteratively solving the palm transformation and finger displacement. The difference is that the finger displacement for a finger case can be represented as instead of the change of jaw width . And the problem can be formulated as
(14a)  
(14b)  
where is the error metric for the th finger, is the initial position of th finger. Similarly to the one DOF case in (9), the problem (14) can be decomposed to a series of palm optimization and finger optimization. The palm optimization optimizes for the optimal palm transformation with fixed , while the finger optimization solves for the optimal finger displacement with fixed .
The derivation of IPFO for multifingered hands is similar with iterative contactpalm optimization for finger splitting, which can be referred to our previous work [14] for more details.
IiiD Initialization and Sampling
The initialization of ISF is important since ISF converges to local optima. Various initialization is desired for the algorithm to explore different regions of the object, so as to avoid getting trapped in bad local optima and achieve better collisionavoiding solutions.
In this paper, the object is firstly partitioned into clusters by kmeans clustering. The center of each cluster is regarded as a candidate initial position of the gripper for ISF, and the initial orientation of the gripper is randomly sampled.
We build an empirical model to guide the sampling among candidates. Similar to the multiarmed bandit model, we record the error of ISF evaluated in each cluster center and compute the average regret accordingly. The average regret of each cluster center is used to guide the succeeding sampling. The sampling process is summarized in Alg. 3.
The Alg. 3 is fed by the surfaces and the parameters including the cluster number , the total sampling times and initial jaw width . The sample centers is generated by kmeans clustering (Line 2). The system stores and to represent the previous sampling and evaluation experience. The sampling is guided by the average regret, and the cluster center with the minimum regret is chosen as the initial position of the gripper for the following ISF, while the initial orientation is randomly sampled (Line 4). The gripper with the sampled initialization is evaluated by ISF and checked collision (Line 5). The is a boolean function that returns 1 when the two input arguments have collision, i.e. two object surfaces have intersection. In Line 6, the average regret of the th sample is updated by considering the surface fitting error and the collision penalty. The is a penalty factor that penalizes the average regret for the collided samples.
Iv Simulations and Experiments
In this section, both the simulation and the experiment results are presented to verify the effectiveness of grasp planning algorithm. The computer we used was a desktop with 32GB RAM and 4.0GHz CPU. All the computations were conducted by Matlab. We used a parallel jaw gripper with the specialized fingers as shown in Fig. 3. The desired contact surfaces are marked by red. The gripper width was constrained by cm.
Iva Parameter Lists
The initial gripper width was set as = 2 cm. The weight for normal penalty was . The convergence threshold was selected as in Alg. 1. The pyramid level , the maximum iteration and the tolerance were set as , and respectively in Alg. 2. The kmeans center number , the sample number , and the collision penalty in Alg. 3.
IvB Simulations
The grasp planning optimized for grasps by guided sampling and ISF evaluation. The proposed guided sampling enabled more efficient exploration by minimizing the average regret based on previous experience. The proposed ISF algorithm considered the surface fitting in different opening by optimizing both the palm transformation and finger displacement.
Figure 4 shows the grasp planning result on an Oscar model. The object and gripper surface were fed into the algorithm as shown by the blue and red dots in Fig. 4(a). The vertices of the object were firstly processed by downsampling and normal estimation, after which the kmeans clustering ran for centers of initialization, as shown by bold dots. Multiple grasps were generated (shown by red patches in Fig. 4(a)) and passed through the collision check function. The collision check function detected whether the gripperobject collision occurred. The planned collisionfree grasp in the figure is represented by the pose of the solid gripper, while the collided grasp is represented by the pose of the transparent one as shown in Fig. 4(b).
Figure 5 shows the surface fitting errors of ISF and one execution of IPFO during the grasp planning for the Oscar model. The average distance between and was dropped from 10 mm to 1 mm by running ISF as shown in Fig. 5(a), where the shaded area is magnified and shown in Fig. 5(b). The red line shows the finger displacement in different iterations and the blue line shows the fitting error. The IPFO could achieve efficient convergence within five iterations.
The simulation results on different objects are visualized in Fig. 6. Some of the objects (e.g. Doraemon and Bunny) were scaled to fit into the gripper range. The simulation details for these objects are shown in Table I. The second column shows the number of collisionfree optimal grasps over the total samples. The third column shows the total computation time (i.e. the time to run Alg. 3) to generate these grasps. The last column shows the number of vertices for each object. The dragon was challenging to grasp due to collision caused by the complex geometry. In average, the grasp planning took 2.33 s to find 36.2 collisionfree grasps in 60 times of samples. Thus, each collisionfree grasp took 64.4 ms in average.
Objects 





Hand  53/60  1.85  1939  
Gun  36/60  2.45  2287  
Car  18/60  1.68  2166  
Oscar  31/60  2.03  3684  
Dragon  3/60  5.61  3971  
Bunny  33/60  1.56  740  
Banana  54/60  3.82  1723  
Screw Driver  58/60  1.71  930  
Doraemon  40/60  1.84  769  
Average  36.2/60  2.33  2022.1 
IvC Experiments
A series of experiments were further performed on a FANUC industrial manipulator to verify the effectiveness of the proposed algorithm. Two Ensenso cameras were used to capture the point cloud of the object. Compared with simulation, the point cloud produced by Ensenso cameras was not able to reflect the object precisely due to occlusion and noise.
Figure 7 shows the grasp planning results on six different objects including three toy robot models and three tools in different sizes. The left side for each subfigure in Fig. 7 shows the grasp optimized by ISF, and the right side shows the success result in lifting the object by 10 cm. Although some of the objects had complicated shapes, ISF could find a grasp that matched the fingertips to the object surface well. Therefore, the robot could firmly grasp the object, and further increased the grasp robustness.
A clutter task experiment was performed and shown in Fig. 8, where several objects were placed closely (Fig. 8(a)). Figure 8(b)(f) show the consecutive grasps in the task. In this experiment, the proposed algorithm directly exploited grasp poses on the unsegmented point cloud by fitting the fingertip surface to the object sets. Even though the surface composed by the cluttered objects became more complicated than a single object, ISF filtered out the grasps with collision and found a suitable grasp to pick up objects sequentially.
V Conclusions and Future Works
This paper proposed an iterative surface fitting (ISF) algorithm to plan grasps for customized grippers. ISF searched for optimal grasps by an iterative palmfinger optimization, which solved for the optimal palm pose and the finger displacement iteratively with closedform solutions. To avoid bad local optima, the guided sampling was introduced to initialize ISF searching. The proposed grasp planning algorithm was applied to a series of simulations and experiments. ISF achieved 64.4 ms average searching time to find a collisionfree grasp on various objects. The grasp planning was further implemented in a clutter task to grasp objects from unsegmented point clouds.
There are several directions that we want to explore in the future. First, we would like to apply the algorithm to different types of grippers and perform the experiments in more practical scenarios. In addition, we would like to extend the algorithm to the grasp planning of general multifingered hands and soft hands, by simultaneously searching for optimal transformation and finger joint configuration. Finally, we would like to create an algorithm to learn the guided sampling by the grasp experience of different objects using the kmeans in feature space.
Acknowledgment
The authors would like to thank Berkeley AutoLab for providing the 3D printed objects.
References
 [1] R. M. Murray, Z. Li, and S. S. Sastry, A mathematical introduction to robotic manipulation. CRC press, 1994.
 [2] M. Cutkosky and P. Wright, “Modeling manufacturing grips and correlations with the design of robotic hands,” in Robotics and Automation. Proceedings. 1986 IEEE International Conference on, vol. 3. IEEE, 1986, pp. 1533–1539.
 [3] M. R. Cutkosky, “On grasp choice, grasp models, and the design of hands for manufacturing tasks,” IEEE Transactions on robotics and automation, vol. 5, no. 3, pp. 269–279, 1989.
 [4] K. Hang, J. A. Stork, and D. Kragic, “Hierarchical fingertip space for multifingered precision grasping,” in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on. IEEE, 2014, pp. 1641–1648.
 [5] M. Ciocarlie, C. Goldfeder, and P. Allen, “Dexterous grasping via eigengrasps: A lowdimensional approach to a highcomplexity problem,” in Robotics: Science and Systems Manipulation WorkshopSensing and Adapting to the Real World. Citeseer, 2007.
 [6] Y. Li, J. L. Fu, and N. S. Pollard, “Datadriven grasp synthesis using shape matching and taskbased pruning,” IEEE Transactions on Visualization and Computer Graphics, vol. 13, no. 4, pp. 732–747, 2007.
 [7] E. Klingbeil, D. Rao, B. Carpenter, V. Ganapathi, A. Y. Ng, and O. Khatib, “Grasping with application to an autonomous checkout robot,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 2837–2844.
 [8] A. ten Pas and R. Platt, “Using geometry to detect grasp poses in 3d point clouds,” in Robotics Research. Springer, 2018, pp. 307–324.
 [9] P. J. Besl and N. D. McKay, “Method for registration of 3d shapes,” in Sensor Fusion IV: Control Paradigms and Data Structures, vol. 1611. International Society for Optics and Photonics, 1992, pp. 586–607.
 [10] T. Jost and H. Hugli, “A multiresolution icp with heuristic closest point search for fast and robust 3d registration of range images,” in 3D Digital Imaging and Modeling, 2003. 3DIM 2003. Proceedings. Fourth International Conference on. IEEE, 2003, pp. 427–433.
 [11] K.L. Low, “Linear leastsquares optimization for pointtoplane icp surface registration,” Chapel Hill, University of North Carolina, vol. 4, 2004.
 [12] T. Zinßer, J. Schmidt, and H. Niemann, “A refined icp algorithm for robust 3d correspondence estimation,” in Image Processing, 2003. ICIP 2003. Proceedings. 2003 International Conference on, vol. 2. IEEE, 2003, pp. II–695.
 [13] Y. Chen and G. Medioni, “Object modelling by registration of multiple range images,” Image and vision computing, vol. 10, no. 3, pp. 145–155, 1992.
 [14] Y. Fan, T. Tang, H.C. Lin, and M. Tomizuka, “Realtime grasp planning for multifingered hands by finger splitting,” IEEE Robotics and Automation Letters (Under Review), 2018.