New Formulation of Mixed-Integer Conic Programming for Globally Optimal Grasp Planning

We present a two-level branch-and-bound (BB) algorithm to compute the globally optimal gripper pose that maximizes a grasp metric. Our method can take the gripper's kinematics feasibility into consideration to ensure that a given gripper can reach the set of grasp points without collisions or predict infeasibility with finite-time termination when no pose exists for a given set of grasp points. Our main technical contribution is a novel mixed-integer conic programming (MICP) formulation for the inverse kinematics of the gripper that uses a small number of binary variables and tightened constraints, which can be efficiently solved via a low-level BB algorithm. Our experiments show that globally optimal gripper poses for various target objects can be computed taking 20-180 minutes of computation on a desktop machine and the computed grasp quality, in terms of the Q1 metric, is better than those generated using sampling-based planners.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

09/12/2019

Globally Optimal Grasp Planning using a Two-Stage Branch-And-Bound Algorithm

We present the first algorithm to compute the globally optimal gripper p...
02/04/2020

Deep Differentiable Grasp Planner for High-DOF Grippers

We present an end-to-end algorithm for training deep neural networks to ...
09/20/2021

Human Initiated Grasp Space Exploration Algorithm for an Underactuated Robot Gripper Using Variational Autoencoder

Grasp planning and most specifically the grasp space exploration is stil...
01/20/2020

Mixed integer programming formulation of unsupervised learning

A novel formulation and training procedure for full Boltzmann machines i...
07/20/2019

Generating Optimal Grasps Under A Stress-Minimizing Metric

We present stress-minimizing (SM) metric, a new metric of grasp qualitie...
04/30/2019

3D Grasp Stability Analysis with Coulomb Friction with Hierarchical Convex Relaxations

We present an algorithm to determine quasistatic equilibrium of three di...
11/20/2019

A simple and efficient dichotomic search algorithm for multi-objective mixed integer linear programs

We present a simple and at the same time fficient algorithm to compute a...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

(a)(b)(c)Formulation in [6]Ours(d)(e)

Fig. 1: (a): A toy example where the target object is a Z-shape (olive) on which we have potential grasp points (red point). (b): We first build a KD-tree for the potential grasp points. Our gripper has links and fingertip points. (c): For any BBNode, e.g., , we will use the IK solver for feasibility check. We allow MICP to use few binary variables by allowing the target object to move and fixing the palm of the gripper. (d): We build a bounding sphere, i.e. and , for each non-leaf KD-tree node. (e): For an internal ( means any KD-tree node), can either be at or . This constraint has a convex relaxation that requires to be inside the intersection of bounding spheres (blue): .

Grasp planning is a well studied problem in robotics and there is large amount of work in grasp metric computation [17] and gripper pose planning [4]. Since the two components are somewhat independent, practitioners can build versatile planning frameworks that allow arbitrary combination of grasp metrics and gripper pose planners for different applications [13]. A high number of choices have been proposed for grasp metrics [17], and a few gripper pose planners are also known. Some planners such as [6, 24] return sub-optimal solutions, which are sensitive to initial guesses and can return grasps of low qualities. Another planner based on simulated annealing (SA) was proposed in [4], which can compute the globally optimal solution if it is allowed to consider an infinite number of samples.

A promising direction of previous works [10, 21] use branch-and-bound (BB) to compute globally optimal grasp points that maximize a given grasp metric. Unlike SA, BB returns the globally optimal solution or predicts infeasibility within a finite amount of computational time. However, BB algorithms in [10, 21] only compute the globally optimal grasp points without considering the gripper’s kinematics feasibility. To take the gripper’s kinematics into consideration, an inverse kinematics (IK) algorithm is needed to determine whether a given set of grasp points can be reached by the gripper. However, most available inverse kinematics algorithms, such as [15, 1], are not globally optimal and can miss feasible solutions when one exists. Recently, a complete IK algorithm is presented in [5], which reformulates IK as a mixed-integer conic programming (MICP). However, [5] involves the use of a large amount of integer parameters making it slow to solve due to the combinatorial worst-case complexity of MICP.

Main Results: We present a novel, two-level BB algorithm to compute the globally optimal gripper pose that maximizes a given grasp metric, where the two levels correspond to the two requirements of optimization. Our contributions include:

  • A low-level BB that uses a novel, compact MICP formulation, leading to much fewer binary decision variables than previous work [5]. Within 1min of computation on a desktop machine, our low-level BB can check whether there exists a gripper’s pose, while the gripper’s end-effectors can reach the given set of grasp points.

  • A high-level BB that searches for a set of grasp points to maximize the grasp metric, ignoring the gripper’s pose. Our high-level BB uses novel mechanics to prune unnecessary computations for low-level BB and further reduce the computational cost.

  • A lazy-MICP formulation that introduces collision constraints progressively, which efficiently resolve collisions between the gripper and the target object.

We have tested our algorithm on a row of 10 target objects grasped by a 3-finger gripper with 15 DOFs and a BarrettHand with 11 DOFs. Our experiments show that globally optimal grasps can be computed within 20-180 minutes on a desktop machine for different grippers. Furthermore, our low-level BB formulation results in a speedup of over [5] in terms of gripper’s kinematics feasibility check. We have also compared our algorithm’s performance with a sample-based grasp planner [4] and observed the following benefits:

  • Our method always computes higher quality grasps based on metric, though we are slower.

  • Our method can detect infeasibility within finite time, which happens frequently when target objects are large compared with the gripper.

Ii Related Work

We review previous works on grasp metric computation, gripper pose planning, and IK algorithms.

Grasp Metrics measure the quality of a grasp and provide ways to compare different grasps. A dozen of different grasp metrics have been proposed and summarized in [17]. Sampling-based planners can be used to optimize all kinds of grasp metrics. However, BB can only be used when a grasp metric is monotonic [10, 21], i.e. the grasp metric value computed for a superset of grasp points must be larger than that computed for a subset. Fortunately, most metrics, including [12], [7], are monotonic. In particular, [6] showed that the computation of can be approximated by solving a semidefinite programming (SDP), allowing the BB to be solved using off-the-shelf mixed-integer SDP solvers [8]. Instead, we propose to use a more generic form of BB-based algorithm for our high-level problem in order to account for many different metric types.

Gripper Pose Planning takes the gripper’s kinematics feasibility into consideration, which computes a griper pose given the set of contact points as end-effector constraints. Some sampling-based planners [4, 13] determine the gripper’s pose first by sampling in the gripper’s configuration space. However, it is rather difficult for the fingers to exactly lie on the surface of target objects, so these planners have to close the gripper to have the fingers on the object surface. Other planners, such as [10] and our method, first select contact points, compute the grasp quality, and then solve the IK problem to compute the gripper’s pose.

IK Algorithms can be used to check kinematics feasibility. However, these algorithms are sub-optimal or sampling-based. An sub-optimal IK algorithm such as [2, 15, 1] can return false negatives, i.e. reporting infeasibility when a solution exists. On the other hand, a sampling-based IK algorithm such as [1] can always find the solution, but assume an infinite amount of samples are used. Recently, a new IK algorithm based on MICP relaxation is proposed in [6], which finds a solution or detects infeasibility in a finite amount of time. However, we found that the formulation of [6] requires too many binary variables, making MICP solve computationally costly due to its combinatorial worst-case complexity.

Iii Problem Statement

In this section, we formulate the problem of grasp planning. All the symbols used in this paper are summarized in table:symbols. As illustrated in fig:pipeline, the input to the planner includes:

  • A target object that occupies a volume .

  • A set of potential grasp points: .

  • A gripper represented as an articulated object, i.e. a set of rigid links. Each link occupies a volume , where and is the set of joint angle and globally transformation parameters. On the gripper, there is a set of fingertip points: and . W.L.O.G., we always assume the first links are fingertip links so that .

  • A grasp metric whose input is a set of grasp points satisfying . Moreover, we assume that the grasp metric is monotonic, i.e. .

In this paper, we assume that the first parameters in are extrinsic parameters ( for rotation and for translation) and the rest are intrinsic parameters, i.e. joint angles. Given these inputs, the planner either predicts that the problem is infeasible or outputs satisfying the following conditions:

  • C1: The gripper does not collide with the target object or have self-collisions. In other words, and .

  • C2: Each fingertip point lies on the object surface, i.e. .

  • C3: For all other satisfying C1 and C2, we have: .

Variable Definition
target object
th link of gripper
th potential grasp point
th fingertip point
normal on /
A set of potential grasp points
The KD-tree node for
parent of KD-tree node /root of tree
left/right child of KD-tree node
gripper’s kinematics parameter
kinematics parameter influencing
th lower/upper bound in
conceptual solution space
minimal bounding sphere/cone
center/radius of
Variable Definition
center/radius of
considering user threshold
number of gripper links
number of fingertip points
number of potential grasp points
number of separating directions
monotonic grasp metric
global rotation/translation of
global rotation/translation of
auxiliary variables
grid index in piecewise approximation
#cells in piecewise approximation
th separating direction

rotation vector of

penetration depth
TABLE I: Symbol Table

Note that previous works [10, 21] ignores C1 and C2 and solve the problem using a one-level BB algorithm. On the other hand, the sampling-based planner [4] solves the full version of this problem by generating samples of and then testing C1, C2, and C3, but SA cannot detect infeasibility. Instead, our two-level BB algorithm takes C1, C2, and C3 into consideration with finite time termination. Conceptually, we identify a large enough subset of the entire solution space. If we restrict ourselves by adding a condition, C4: , then the globally optimal solution to the planning problem can be efficiently solved within a finite amount of computational time.

Iv Two-Level Branch-and-Bound Formulation

We solve the problem stated in sec:problem using a two-level BB algorithm. It is known that BB can efficiently find the globally optimal solution for non-convex optimization problems [14] if a relaxation to the problem is available, which allows pruning sub-optimal solutions at an early stage. In our high-level BB, the relaxation is provided by the monotonicity of the grasp metric. In our low-level BB, the relaxation is provided by turning the integer variables into continuous variables.

Iv-a High-Level BB

Our high-level BB takes a very similar form as [10]. We select points, , from the set of potential grasp points, , such that the grasp quality is maximized. To solve this problem, we first build a KD-tree for . As illustrated in fig:pipelineb, each KD-tree node is uniquely denoted by a subset .

The BB algorithm finds the globally optimal solution by building a search tree and keeping track of the best solution with the largest grasp quality metric found so far, which is defined as . Each node on the search tree can be uniquely denoted by , where each is the KD-tree node for the th fingertip point. This is also the set of potential grasp points that can possible be at. At each BBNode, we encounter one of the two cases:

  • If for all , then the BBNode is a leaf node and we compute tentative grasp quality for this node: . If the tentative grasp quality is larger than , then this BBNode is known as an incumbent and is updated.

  • If there is an such that , then the BBNode is a non-leaf node. In this case, we also compute the tentative grasp quality for this node. If the tentative grasp quality is smaller than then this BBNode is eliminated for further processing. Otherwise, we branch on all the with .

It has been shown in [10] that this algorithm will find the globally optimal if is monotonic. However, this previous work does not take gripper’s kinematics feasibility into consideration. Each BBNode essentially specifies all the possible positions of each fingertip point. However, if it is impossible for the gripper to reach these positions, then the given BBNode does not contain feasible solutions and should be cut early to avoid redundant search.

Iv-B Gripper’s Inverse Kinematics

Before we discuss feasibility checks of BBNode, we first propose a novel, MICP-based globally optimal IK algorithm, which is the cornerstone of our feasibility check algorithm. Compared with [6], which can be applied to solve IK for any articulated robot, our formulation only works for the problem of gripper pose planning but uses much fewer binary variables, leading to significant speedup.

As illustrated in fig:pipelinec, our main idea is that applying a global transformation of the gripper is equivalent to applying a global inverse transformation of the target object while keeping the palm of gripper fixed. However, if we keep the palm of gripper fixed, then the fingers of the gripper become decoupled. Specifically, we assume that each fingertip such that: . This assumption holds if we allow the target object to have a global rigid transformation.

Based on this assumption, we can relax the IK problem as MICP. Specifically, we introduce auxiliary variables for the rotation and translation of the th link. The main constraint to relax is where is also a function of . We relax using a piecewise linear approximation by introducing the following constraints:

(1)

where is the special ordered set of type 2 [22] and are continuous-valued auxiliary variables. This piecewise linear approximation restricts the solution space, which corresponds to our last condition C4 in sec:problem. The mixed-integer constraints in eq:XChoose require constraints and hence binary decision variables. Finally, is defined as:

(2)

where and are joint limits. In other words, we build a -dimensional grid with cells along each dimension. Next, We discretize on the grid and use mixed-integer constraints to ensure that falls inside only one of the cells. Note that all the are precomputed using forward kinematics and used as coefficients of the linear constraints (eq:XChoose).

Since the palm of gripper is fixed, we have to inversely transform the target object. As a result, each potential grasp point can be transformed into where . The technique to relax as MICP has been presented in [6] but this technique requires too many binary decision variables. Instead, we use a similar technique as eq:XChoose. Based on the Rodrigues’ formula , where is an arbitrary 3D vector, we introduce the following MICP constraints:

(3)

which requires binary decision variables and are continuous-valued auxiliary variables. Given these constraints, the requirement that the th fingertip point is at can be formulated as a linear constraint:

(4)

In summary, we reduce the IK problem for the gripper to a set of linear constraints, whose feasibility can be efficiently verified using off-the-shelf solvers such as [9]. Putting the two parts together, our formulation needs binary decision variables to solve the IK problem.

Iv-C Low-Level BB

The goal of solving low-level BB is to check whether a contains a feasible solution in terms of gripper’s kinematics. In sec:IK, the IK problem is formulated as a MICP. However, solving IK is not enough for feasibility checks of BBNodes because eq:EECons constrains that each can only be at one point, while a BBNode generally allows to be at one of several points in non-leaf cases. In the later case, we have at least one so that can be at any point in the set . In order for the feasibility check to be performed using the off-the-shelf MICP solver [9], we have to relax this point-in-set constraint as a linear or conic constraint. A typical relaxation is to constrain that lies in the convex hull of the set. However, this constraint takes the following form which is not convex:

(5)

This is because and are both variables, leading to a bilinear form. It is possible to relax a bilinear form into MICP by requiring additional binary decision variables. Instead, we propose to construct a minimal bounding sphere for the set denoted as:

where is the center of sphere and is the squared radius. Next, we relax the point-in-set constraint as:

(6)

which is a quadratic cone and can be handled by [9]. Note that and are constants and can be precomputed for each node of the KD-tree (see appen:bound for details). A minor issue is that eq:BCons is not as tight as eq:CCons in terms of the volume of the constrained space. To alleviate this problem, we notice that if has a parent in KD-tree denoted as , then should also satisfy the point-in-set constraint of . Therefore, we can backtrace to the root KD-tree node and add all the constraints of eq:BCons along the path, as illustrated in fig:pipelinee.

(a)(b)

Fig. 2: (a): We illustrate the normal of fingertip points (white arrow) and the inward normal of potential grasp points (black arrow). We allow to lie in a normal cone around (blue) with a threshold (dashed line). (b): We illustrate the relaxed normal cone of the two potential grasp points (red) with threshold denoted as . The final threshold used in the constraint is , taking the user defined threshold into consideration. Note that all vectors have unit length and we use an extruded red region for the clarity of figure.

Iv-D Normal Constraints

We can further optimize our formulation by taking the surface normals of the target object into consideration, leading to even tighter constraints. As illustrated in fig:normala, each potential grasp point can be associated with an inward surface normal denoted by . Also, each fingertip point can also be associated with a normal . It is intuitive to constrain that should be pointing at a similar direction to . In practice, we do not need to align with exactly, but allow to lie in a small vicinity. Therefore, if a leaf is encountered, then we add the following constraint to MICP for each :

(7)

where is a user-defined threshold. If a non-leaf BBNode is encountered, then we have to add a normal-in-set constraint. Using a similar technique as sec:low, we construct a normal cone denoted as:

for each internal KD-tree node during precomputation. Here is the central direction of the normal cone and is the squared radius. We can then add the relaxed normal-in-set constraint for :

(8)

where is the squared radius of the normal cone taking the user-defined threshold into consideration, as illustrated in fig:normalb (see appen:bound for details). Finally, we can further tighten the normal-in-set constraint using a similar technique as sec:low. We can backtrace to the root KD-tree node and add all the constraints of eq:NConsR along the path.

Iv-E Collision Handling using Lazy-MICP

In addition to checking gripper’s kinematics feasibility, our low-level BB also ensures that gripper’s links do not collide with each other or with the target object. It has been shown in [20, 6] that collision constraints can be relaxed as MICP. In order to reduce the use of binary decision variables, we propose to add collision constraints in a lazy manner.

Specifically, we assume that the target object and all gripper links are convex objects. If is not convex then we can approximate it using a union of convex shapes. We first ignore all collision constraints and solve MICP. We then detect collisions between and and record the pair of points with deepest penetration denoted as , e.g. using [11]. If we find that and are in collision, then we pick a separating direction from a set of possible separating directions and introduce the following constraint as illustrated in fig:collisiona and fig:collisionb:

(9)

where is the special ordered set of type 1 [22], are the auxiliary variables, and is the big-M parameter [19]. Similarly, if there is a collision between a pair of points, and , then we have the following constraint:

(10)

After adding collision constraints, the new MICP is solved again with a warm-start and we again perform collision-detection. This is looped until no new collisions are detected or MICP becomes infeasible. Note that if a new collision is detected for a link-link or link-object pair for which collision has been detected in previous loops, then only the first lines of eq:collA and eq:collB are needed. In other words, binary decision variables are needed once for each link-link and link-object pair and the number of decision variables is .

Note that the collisions between the first K fingertip links and the target object does not need to be detected or handled by MICP. This is because each fingertip link contacts the target object at one point with matched normal when eq:NCons holds with , which is a sufficient condition for two convex objects to be collision-free [18]. In practice, we allow users to set a small, positive to account for inaccuracies in gripper and target object shapes.

(a)(b)(c)

Fig. 3: An illustration of our collision handling algorithm. (a): There is collision between Link3 and the target object. (b): MICP selects one of the 8 possible separating directions. (c): Collision can be resolved when is selected (red). MICP does not need to consider the collisions between Link1, Link2 and the target object because they contact at one point with matched normal.

V Algorithm Acceleration

Our method discussed in sec:formulation is computationally costly due to the repeated use of the MICP-based IK algorithm, to check the kinematics feasibility of the gripper. In this section, we discuss three techniques to reduce the cost of MICP solve. Our first technique is bottom-up kinematics check, which is based on the following observation:

Lemma V.1

If the MICP-based IK problem for a BBNode is feasible, then the MICP-based IK problem for its parent is also feasible.

Therefore, we can check the gripper’s kinematics feasibility lazily. Specifically, if a BBNode is a non-leaf node and it has not been checked for gripper’s kinematics feasibility, then we skip the check and continue branching. If a BBNode is a leaf node, then we solve MICPs to check for gripper’s kinematics feasibility for all the BBNodes on the path between this leaf node and the root BBNode. If any of the MICP appears to be feasible in this process, then all the ancestor nodes will also be feasible and their checks can be skipped. Our second technique is warm-started MICP solve. We propose to store the solution of MICP for each BBNode and use this solution as the initial guess for the MICP solves of its children. Our third technique is local optimization. Note that MICPs are convex relaxations of non-convex optimization problems. Non-convex optimization problems are sub-optimal but efficient to solve. Therefore, we propose to solve a non-convex optimization before invoking a call to the MICP solver. If the non-convex optimization appears to be feasible, then we can skip the MICP solves. In practice, we use interior point algorithm [3] as our non-convex optimization solver. The key steps of our algorithm is illustrated in fig:steps and we summarized our method in appen:algor.

(a)(b)(c)(d)

Fig. 4: We illustrate key steps of our algorithm. (a): We skip MICP-based IK checks for non-leaf BBNodes. (b): We solve MICP for BBNodes in a bottom-up manner. If a BBNode is infeasible (red), then the feasibility of its parent BBNode must be checked by solving another MICP. If a BBNode is feasible (green), then its parent must be feasible and we can skip the check. (c): Another leaf BBNode is cut due to the infeasibility of its parent. (d): The MICP solve on a BBNode can be warm-started from a parent BBNode.

Vi Experiments and Results

We perform all the experiments on a single desktop machine with one Intel I7-8750H CPU (6-cores at Hz). Given a target object, we first sample on using parallel Poisson disk sampling [23] and then build a KD-tree for the set of points using [16]. Finally, we solve low-level MICP problems using [9]. To grasp the object, we use a 3-finger axial-symmetric gripper with and . Each finger of the gripper is controlled by one ball joint and one hinge joint. Under this setting, our IK formulation requires binary decision variables while [6] requires binary decision variables. The average solve time using our formulation and [6] are compared in table:IKCompare, which indicates that our formulation is over more efficient.

#Binary Ours #Binary [6] Ours(s) [6](s)
2 12 630 0.034s 23.021s
4 24 1260 1.231s 287.741s
8 36 1890 48.366s 8632.237s
TABLE II: Our MICP-based IK formulation is over faster than [6] because we use very few binary decision variables. From left to right: number of pieces in discretization, #binary decision variables using our formulation, #binary decision variables using [6], our average solve time, and the average solve time of [6] (50 random trials).
Fig. 5: We show a list of optimal grasps found for the 3-finger axial-symmetric gripper and a row of 6 objects. For some objects we show two different grasps, one for a large gripper and the other for a small gripper. Finally, we plot the convergence history and computational cost of different substeps of our algorithm for the cabinet model.

A list of results are demonstrated in fig:mainResults and we show the convergence history for one instance. In all these examples, we choose . Under this setting, our algorithm needs to explore K BBNodes in order to find the globally optimal grasp and the computation takes 20-180 minutes depending on the complexity of target object shapes. We also plot the computational cost of different substeps of our algorithm, where 65 of the BBNodes are cut due to incumbent or gripper’s kinematics infeasibility, MICP solves are only needed by 1.9 of the BBNodes, and local optimization can be used to avoid MICP solves need by 0.1 of the BBNodes. Finally, if we ignore the low-level BB and only run the high-level BB, then our algorithm coincides with [10], which only searches for a set of grasp points. The computation corresponding to only high-level BB takes less than minutes. Therefore, the main bottleneck of our algorithm is the gripper’s kinematics check, or the low-level BB.

Fig. 6: We plan grasps with the target object being a tuning fork and rescale the object by times. As the scale of the object grows, the optimal grasp quality reduces and the gripper can only grasp a smaller part of the target object, leading to lower grasp quality.

In fig:mainResults, we also show two grasps for some objects using a large and a small gripper. The large gripper can hold the entire object. But if the gripper is small, it can only hold a part of the target object. A more systematic evaluation is shown in fig:scale, where the quality monotonically decreases as we use larger version of the same objects.

Finally, we compare performance of our method with a sampling-based method fig:advantage using both the 3-finger gripper and the 11-DOF BarrettHand. Being incomplete, sampling-based method sometimes cannot find solutions, especially when the target object is large compared with the gripper. This is because feasible grasps become rare in the configuration space when object size grows and most samples are not valid. As a result, the initial guess of the gripper’s pose is important when using [13]. However, our method can always find a solution when one exists and we do not require users to provide an initial guess. Even when sampling-based method can find a solution, our solution always has a higher quality in terms of the value of metric. On the other hand, [13] can find a sub-optimal solution within 10min which is faster than our method. However, if only sub-optimal solutions are needed, user can choose to terminate our algorithm when is larger than a threshold. According to the convergence history in fig:mainResults, our method can usually find feasible solutions after exploring K nodes, which also takes several minutes.

Vii Conclusion & Limitations

We present a two-level BB algorithm to search for the globally optimal grasp pose maximizing a given monotonic grasp metric. The high-level BB selects grasp points that maximize grasp quality, while the low-level BB cut infeasible BBNodes in terms of gripper’s kinematics. Our low-level BB uses a compact MICP formulation that requires a small number of binary variables. Experiments show that our method can plan grasps for practically complex objects.

(a): [13]Ours(b): [13]Ours

Fig. 7: We show the advantage of our method over sampling-based method [13]. (a): Sampling-based method can mostly find a solution when an object is small, though the grasp quality is less than our solution. (b): When the object is large, sampling-based method sometimes cannot find a solution, while our method always finds solutions when one exists.

Our work has several limitations, which leave spaces for avenues of future work. First, we only plan gripper poses without consider other sources of infeasibility such as environmental objects and robot arms. When robot arms are considered, the decoupled assumption of sec:IK is violated and we need new techniques for relaxing IK as MICP. Second, although our IK relaxation is more efficient than [6], our method is not an outer-approximation. In other words, if a gripper pose is feasible using exact IK, it might not be feasible under our relaxed IK constraints. In addition, our formulation incurs high computational cost for complex object shapes, such as those acquired from scanning real-world objects, which requires many convex shapes to approximate it and many binary decision variables to formulate the collision constraints.

Viii Appendix: Bounding Spheres & Cones

For each KD-tree node , the globally minimal bounding sphere can be computed by solving the following conic programming problem:

If each has an attached normal , we can compute a minimal bounding cone by solving the following non-convex programming problem:

which is non-convex due to the unit length constraint . To compute the globally minimal bounding cone, we relax the unit length constraint using MICP via the technique presented in [6]. Finally, we take the user-defined threshold into consideration and compute as follows:

Ix Appendix: Algorithms

A summary of algorithms. Given a gripper and a target object, we first perform a precomputation using Alg:pre. Afterwards, we use Alg:high as high-level BB and use Alg:low as low-level BB. The accelerated bottom-up kinematics check is summarized in Alg:BUCheck, which is used as a middleware between the two levels.

1:Sample on using [23]
2:Construct KD-tree for using [16]
3:for Each in KD-tree do
4:     Construct and (appen:bound)
5:for Each link on the gripper do
6:     Construct relaxed IK constraints (eq:XChoose)
7:Construct relaxed IK constraint for (eq:RChoose)
Algorithm 1 Precomputation
1: Check if ancestor BBNode.LowLevel=False
2: If so, cut early due to Lem:Feas
3:CurrentBBNodeBBNode
4:do
5:     if CurrentBBNode.LowLevel=False then
6:         BBNode.LowLevel=False
7:         Return      
8:     CurrentBBNodeCurrentBBNode.Parent
9:while CurrentBBNode is not the root
10: Bottom-up Kinematics Check
11:CurrentBBNodeBBNode
12:ChildFeasibleFalse
13:do
14:     if CurrentBBNode.LowLevelUnknown then
15:          The BBNode and ancestors have been checked
16:         Break
17:     else if ChildFeasible=True then
18:          Must be feasible due to Lem:Feas
19:         CurrentBBNode.LowLevel=True
20:     else
21:          Use MICP to check kinematics feasibility
22:         CurrentBBNode.LowLevel=
23:          LowLevelBB(CurrentBBNode)      
24:     ChildFeasibleCurrentBBNode.LowLevel
25:     CurrentBBNodeCurrentBBNode.Parent
26:while CurrentBBNode is not root
Algorithm 2 BottomUpKinematicsCheck(BBNode) as described in sec:acceleration
1:
2: BBNode.LowLevel marks gripper’s feasibility
3: BBNode.LowLevel initializes to Unknown
4:, .insert()
5:while  do
6:     .pop()
7:     if BBNode is leaf then
8:         BottomUpKinematicsCheck(BBNode)
9:         if BBNode.LowLevel=True then
10:               Bound
11:              
12:              if  then
13:                                          
14:     else if BBNode.LowLevel=TrueUnknown then
15:          Branch
16:         Find
17:         .insert()
18:         .insert()      
19:Return
Algorithm 3 HighLevelBB
1:
2:MICP.add(eq:XChoose,3)
3:for  do
4:     for Each from to  do
5:         if  then
6:              MICP.add(eq:EECons,7) for
7:         else
8:              MICP.add(eq:BCons,8) for               
9:do
10:      Warm-start if possible
11:     Solve MICP using [9]
12:     if MICP.Feasible=False then
13:         Return False      
14:     Detect penetration among and
15:     if  then
16:         MICP.add(eq:collA or eq:collB)      
17:while 
18:Return True
Algorithm 4 LowLevelBB()

References

  • [1] D. Berenson, S. Srinivasa, and J. Kuffner, “Task space regions: A framework for pose-constrained manipulation planning,” The International Journal of Robotics Research, vol. 30, no. 12, pp. 1435–1460, 2011. [Online]. Available: https://doi.org/10.1177/0278364910396389
  • [2] S. Buss, “Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods,” IEEE Transactions in Robotics and Automation, vol. 17, 05 2004.
  • [3] R. H. Byrd, J. Nocedal, and R. A. Waltz, “K nitro: An integrated package for nonlinear optimization,” in Large-scale nonlinear optimization.   Springer, 2006, pp. 35–59.
  • [4] M. Ciocarlie, C. Goldfeder, and P. Allen, “Dexterous grasping via eigengrasps: A low-dimensional approach to a high-complexity problem,” in Robotics: Science and Systems Manipulation Workshop-Sensing and Adapting to the Real World.   Citeseer, 2007.
  • [5] H. Dai, G. Izatt, and R. Tedrake, “Global inverse kinematics via mixed-integer convex optimization,” The International Journal of Robotics Research, p. 0278364919846512, 2019.
  • [6] H. Dai, A. Majumdar, and R. Tedrake, “Synthesis and optimization of force closure grasps via sequential semidefinite programming,” in Robotics Research.   Springer, 2018, pp. 285–305.
  • [7] C. Ferrari and J. Canny, “Planning optimal grasps,” in Proceedings 1992 IEEE International Conference on Robotics and Automation, May 1992, pp. 2290–2295 vol.3.
  • [8] T. Gally, M. E. Pfetsch, and S. Ulbrich, “A framework for solving mixed-integer semidefinite programs,” Optimization Methods and Software, vol. 33, no. 3, pp. 594–632, 2018. [Online]. Available: https://doi.org/10.1080/10556788.2017.1322081
  • [9] L. Gurobi Optimization, “Gurobi optimizer reference manual,” 2018.
  • [10] K. Hang, J. A. Stork, N. S. Pollard, and D. Kragic, “A framework for optimal grasp contact planning,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 704–711, April 2017.
  • [11]

    Y. J. Kim, M. C. Lin, and D. Manocha, “Incremental penetration depth estimation between convex polytopes using dual-space expansion,”

    IEEE Transactions on Visualization and Computer Graphics, vol. 10, no. 2, pp. 152–163, March 2004.
  • [12] Z. Li and S. S. Sastry, “Task-oriented optimal grasping by multifingered robot hands,” IEEE Journal on Robotics and Automation, vol. 4, no. 1, pp. 32–44, Feb 1988.
  • [13] A. T. Miller and P. K. Allen, “Graspit! a versatile simulator for robotic grasping,” IEEE Robotics Automation Magazine, vol. 11, no. 4, pp. 110–122, Dec 2004.
  • [14] D. R. Morrison, S. H. Jacobson, J. J. Sauppe, and E. C. Sewell, “Branch-and-bound algorithms,” Discret. Optim., vol. 19, no. C, pp. 79–102, Feb. 2016. [Online]. Available: http://dx.doi.org/10.1016/j.disopt.2016.01.005
  • [15] R. M. Murray, A mathematical introduction to robotic manipulation.   CRC press, 2017.
  • [16] S. Popov, J. Gunther, H.-P. Seidel, and P. Slusallek, “Experiences with streaming construction of sah kd-trees,” in 2006 IEEE Symposium on Interactive Ray Tracing.   IEEE, 2006, pp. 89–94.
  • [17] M. A. Roa and R. Suárez, “Grasp quality measures: review and performance,” Autonomous robots, vol. 38, no. 1, pp. 65–88, 2015.
  • [18] R. T. Rockafellar, Convex analysis, ser. Princeton Mathematical Series.   Princeton, N. J.: Princeton University Press, 1970.
  • [19] J. Samuel and K. J. T. Pizzo, Iterative methods for nonlinear optimization problems.   Prentice Hall, Englewood Cliffs, 1972.
  • [20] T. Schouwenaars, B. De Moor, E. Feron, and J. How, “Mixed integer programming for multi-vehicle path planning,” in 2001 European Control Conference (ECC), Sep. 2001, pp. 2603–2608.
  • [21] J. D. Schulman, K. Goldberg, and P. Abbeel, “Grasping and fixturing as submodular coverage problems,” in Robotics Research.   Springer, 2017, pp. 571–583.
  • [22] J. P. Vielma and G. L. Nemhauser, “Modeling disjunctive constraints with a logarithmic number of binary variables and constraints,” Mathematical Programming, vol. 128, no. 1-2, pp. 49–72, 2011.
  • [23] L.-Y. Wei, “Parallel poisson disk sampling,” in ACM Transactions on Graphics (TOG), vol. 27, no. 3.   ACM, 2008, p. 20.
  • [24] Xiangyang Zhu and Jun Wang, “Synthesis of force-closure grasps on 3-d objects based on the q distance,” IEEE Transactions on Robotics and Automation, vol. 19, no. 4, pp. 669–679, Aug 2003.