Grasping as Inference: Reactive Grasping in Heavily Cluttered Environment

A grasping problem can be expressed as estimating current grasp distribution conditioned on the history of observations as $p(g_{t}|z_{0:t},v_{0:t}=1)$ where $\cdot_{a:b}$ is history of variables from time step $a$ to $b$, $g_{t}$ is the grasp as defined in Sec. III, $z_{0:t}$ is history of observation from time step 0 to t, and $v_{0:t}$ is binary success variable which is 1 if task is success and 0 otherwise, which is also introduced in [8]. Note that $p(\cdot|v=1)$ is simplified to $p(\cdot|v)$, because we do not consider cases conditioning on failure. In this paper, the grasp estimation problem is formulated as an inference in HMM as shown in Fig. 3(a). We assume that $g_{t}$ is dependent on prior grasp $g_{t-1}$ because we can reasonably assume there is an optimal grasp distribution of the scene in terms of the grasp success metric, so the current belief state of the grasp is closely related to the prior one. And we set grasp success variable $v_{t}$ dependent to $g_{t}$ and $z_{t}$. Then, the belief state of $g_{t}$, defined as $B(g_{t})=p(g_{t}|v_{0:t},z_{0:t})$, can be propagated from prior time step as,

where $\mu$ is normalization factor. This formulation has a recurrent structure and it is similar to Bayes filter except that the motion model (or transition model) $p(g_{t}|g_{t-1})$ does not involves action and measurement model is expressed as $p(v_{t}|g_{t},z_{t})$ by introducing $v$. That means well-established tools for estimation problems can be utilized to solve this. To apply estimation toolset, motion model $p(g_{t}|g_{t-1})$ and measurement model $p(v_{t}|g_{t},z_{t})$ should be identified first. We reasonably assume that $p(g_{t}|g_{t-1})$ follows Gaussian distribution as noise model because the scene can be regarded as temporarily static between time step $t-1$ and $t$. On the other hand, measurement model $p(v_{t}|g_{t},z_{t})$ can be obtained by calculating $Q$. In the next section, the realization method of recurrently solving (1) is explained.

As explained before, grasp estimation can be formulated as Bayes filter, but it includes highly non-linear models, making the realization not trivial. Among the possible realizations, we use particle filter[5], because it has been verified to work well with non-linear model[17, 18, 19]. The resulting algorithm is GraspPF, which is illustrated in Fig. 3(b) and Alg. 1, and a detailed description follows. The first step is the initialization of the successful grasp distribution. The belief state of grasp $B(g_{t-1})$ is approximated to $M$ particles in grasp configuration space $\{g_{t-1}\}^{M}$ defined in Sec. III, where $\{\cdot\}^{X}$ means batch of size $X$. Then time update is applied to these particles with $\int p(g_{t}|g_{t-1})B(g_{t-1})dg_{t}$, resulting in $\{g^{\prime}_{t-1}\}^{M}$. This transition is easily implemented by noise sampling for each particle. Specifically, we apply Gaussian noise to each $d$, $p^{W}_{P}$, and $R^{W}_{P}$. However, the transition through noise distribution makes the particles violate a constraint, where $p^{W}_{P}$ should be on the surface of the scene. To resolve it, we project each $p^{W}_{G}$ of particles to the scene mesh based on current depth observation included in $z_{t}$. Concretely, in the grasp space, the $p^{W}_{P,i}$ of the particle is projected to $\bar{p}^{W}_{P,i}$, which is the observed points following line of sight, then the batch of particles becomes $\{\bar{g}_{t}\}^{M}$. Afterward, the particles are evaluated by the approximated grasp quality measure function $Q$, which is conditioned on the current observation $z_{t}$. Then the current belief state $B(g_{t})$ is obtained from the resampling based on the evaluation values $\{Q\}^{M}$. These procedures are repeated during the approaching motion, and when the target is close enough, the scripted closing motion is executed.

Throughout the GraspPF process, the past information is kept in prior distribution $B(g_{t-1})$ in the form of particles, and they are refined with continuously received observations. It makes GraspPF entertain sequential information while the measurement model only sees current observation. It is also worth noting that this process can be used as iterative refinement with a fixed observation (i.e. open-loop), which can resolve the roughness of the initial distribution. We demonstrate it in the comparison experiments on the real robot as explained in Sec. V-C. However, to get this inference framework to work, there are two unresolved problems: evaluation and initialization. Let us explain each problem and what makes them challenging.

First, the evaluation problem can be regarded as obtaining $Q$. To obtain exact results, the oracle information of scene and gripper is needed, such as the mesh and pose of each object. Unfortunately, this information is hard to be accessible with real robot settings, so many works adopt bypass methods to predict it with implicit information from the RGBD pixel. This can be achieved by learning-free methods such as [20], but these are hard to be extended to partial depth observation and are also challenging to be computationally efficient, which can be achieved by data-driven methods[8, 10, 12].

On the other hand, the reasonable initial grasp distribution is also essential to the performance since approaching motion takes only a few seconds, limiting the number of iterations in GraspPF to relatively small number. However, the initialization is not trivial because grasp space is continuous, and the result should sufficiently cover the workspace. In this paper, we resolve these evaluation and initialization problems simultaneously by introducing a novel network design, which can not only evaluate grasp candidates but also generate them efficiently, whereas authors in [10, 8] introduce additional network or antipodal heuristics for sampling. In the next section, we explain the details of the network.

Before starting a detailed explanation of the network, we sum up the necessary properties for the network model in GraspPF, which are mentioned in the previous section. First, it should be computationally efficient enough for a closed-loop to refine grasp during approaching motion. Second, it should be able to evaluate any grasp in a bounded space, which is necessary for the measurement model in GraspPF. Lastly, it should be able to generate grasp candidates efficiently to get a reasonable initial distribution of grasps. We found that these can be achieved by a novel concept of the directional grasp quality.

Directional grasp quality is defined as grasp quality conditioned on fixed grasp rotation $R^{W}_{G}$. To utilize this concept, we design DGQ-CNN to predict the pixel-wise directional grasp quality from input as RGBD image and fixed rotation $R^{W}_{G}$ as Fig. 4. We first express grasp rotation as zxy-Euler parameters as $R^{C}_{G}=R_{z}(\alpha)R_{x}(\beta)R_{y}(\gamma)$, where $R^{C}_{G}$ is rotation matrix of $G$ with respect to camera frame $C$, $R_{z}(\alpha)$ is rotation matrix through an angle $\alpha$ with respect to $x$ axis, and so does $R_{x}(\beta)$ and $R_{y}(\gamma)$. Then, $\beta,\gamma$ are included in the inputs of the parameterized CNN, and the $\alpha$ is applied to the RGBD image, which is rotated by $-\alpha$ inspired by [21]. Additionally, the grasp configuration also contains continuous grasp depth $d$ and grasp width $w$, and we set $d$ as an additional input of the CNN and make $w$ as output with the discretization of size 2. In the perspective of CNN, it receives the rotated images and $\beta,\gamma,d,\xi$, and predicts pixel-wise grasp quality for 3 levels, object mask, and valid collision mask per two grasp width $w$, and then total output channel is 6 (3+1+2). Finally, the pixel-wise directional grasp quality is calculated by network outputs after post-processing, which contains extracting the continuous final grasp quality values and Gaussian filter. It is worth specifying that the DGQ-CNN lessens the sampling burden by deleting $p^{W}_{P}$ in the sampling space. To be specific, DGQ-CNN receives only the rotation component $R^{W}_{P}$ in $P$ except $p^{W}_{P}$, and $p^{W}_{P}$ can be reconstructed by the point cloud and pixel index operation.

Once DGQ-CNN is obtained, the sampling and evaluation are straightforward. In the sampling, the rotation $R^{W}_{G}$ and grasp depth $d$ are sampled uniformly within the predefined bound, and they enter into DGQ-CNN, which generates the pixel-wise quality prediction $Q$. Then the grasp candidates are recovered by pixel indices where $Q$ is over the threshold. Finally, the grasps are sampled proportionally in terms of grasp quality $Q$. On the other hand, regarding the evaluation, $Q$ can be obtained from the index-gathering after pixel-wise grasp quality prediction by DGQ-CNN with input as grasp configuration and observation $z$. Therefore introducing DGQ-CNN resolves the initialization and evaluation issue simultaneously. Next, details about the dataset and training for DGQ-CNN are explained.

To generate the dataset for training DGQ-CNN, a labeling method is needed to measure grasp quality (or how successful the grasp is). With the results from [8, 11, 10], we utilize synthetic labeling because directional grasp quality per pixel needs massive evaluations, and it is not accessible to real grasp[14] or human labeling[21]. We use the force-closure based metric motivated by [12, 8, 3], where a prediction models trained on that metric are verified to perform well, and particularly a validation experiment with real grasping is conducted in [3]. Although some works such as [22] report opposite results, we find that it also works well in our case. Specifically, grasp quality for each pixel has 3 dimensions, which correspond with 3 levels of grasp quality. Also, the label contains object mask and valid collision results per width evaluated by the simplified gripper in Fig. 2, and then the final labels have 6 channels as shown in Fig. 5.

The labeling method above can be applied to a real dataset where 3D mesh and RGBD observation are carefully matched, for example, [23, 24], and this approach is used in [12, 3]. However, the sole source of the real dataset is insufficient to train DGQ-CNN because it needs vision data at various distances to work well even at close proximity to objects. So we generate additional data from various distances to the scene and camera parameters by utilizing synthetic rendering and simulation[25]. As previously studied in [16], various object set is also important, so we use multiple sources[26, 23, 27]. A total of 2.1M synthetic and 400K real RGBD pairs are used.

After generating the dataset, the parameterized CNN in DGQ-CNN can be trained with binary classification loss. However, the dataset has a critical problem, sparse segmentation. It means that the positive pixels in the pixel space are too sparse, then the naive binary cross-entropy loss causes the well-known pain, reducing training stability and performance [28]. Therefore we solve it by utilizing the hierarchical structure of the label. We decompose probability of grasp success, conditioned on pixel index and additional inputs of CNN as $\Theta$, into the three conditional probabilities as,

where $\delta_{ij}$ is pixel in index $i$ and $j$, $m_{o}$ is binary random variable of object presence, and $m_{c}$ is binary random variable for valid collision. Then we make DGQ-CNN predict each conditional probability $p(v|m_{c},m_{o},\delta_{ij},\Theta)$, $p(m_{c}|m_{o},\delta_{ij},\Theta)$ and $p(m_{o}|\delta_{ij},\Theta)$, and this decomposition reduce imbalance problem significantly.

In the experiment, we set two layers of hierarchy as Fig. 6: high-level controller and low-level controller. First, the high-level controller runs on 8Hz, which contains GraspPF to estimate successful grasp, and the decision-maker, which decides on high-level motion semantics such as move-to-target or move-to-box. And the low-level controller includes target follower and admittance control. The simplest way to make a gripper approach to the grasp target is by calculating the vector from the gripper to the grasp target in the workspace and moving the robot arm in that direction. However, it does not consider the gripper approach direction and causes a collision between the gripper tip and the object. To prevent that, we design a specific target following controller considering the approaching direction. It is based on the waypoints, and the details are skipped because it is out of the scope of this paper.

In the real robot implementation with the proposed framework and heavily cluttered environment, there are cases where the robot cannot reach the target due to kinematic constraint or self-collision. To resolve this reachability issue, we split $Q$ into two sub-functions, grasp quality measure for robot $Q^{R}$ and scene $Q^{S}$. $Q^{S}$ evaluates grasps based on collision between scene and gripper and grasp quality with a target object, which can be predicted by DGQ-CNN as explained in Sec. IV-C.

In the case of $Q^{R}$, which evaluates grasps in terms of kinematics and self-collision, it is relatively easy to obtain because the calculation of reachability needs the occupancy and configuration of the robot, which can be accessible precisely even in the real robot. Therefore, it can be calculated without any approximation, but empirically the methods based on collision-check and inverse kinematics (IK) are not feasible to check all particles $\{g\}^{M}$ in real-time when $M$ is large. Therefore, we introduce a simple neural network to approximate $Q^{R}$, in which data is generated by simulator[25]. Regarding training details, the states in the workspace are sampled uniformly within bound, and then they are labeled based on IK and self-collision results. Afterward, the simple 2 layers Multilayer Perceptron (MLP) is trained with binary classification loss. The final reachability network only takes 100 micro-seconds to check 1500 grasp candidates, and its accuracy is 97%.

In this section, we present experimental results to quantitatively and qualitatively verify the performance of GraspPF and DGQ-CNN. First, to compare the prediction performance of DGQ-CNN with our grasp quality intuition, the grasp quality prediction results per direction are visualized in Fig. 7. In the prediction example of the first row, a box-shaped object is placed at an angle. Then, the high-quality region at the grasp direction, which matches the inclined angle, has a larger area than in other directions (second column). Similarly, in the second example, the high-quality region varies from left to right following the transition of the y-Euler angle. These examples show that additional grasp rotation input can change high-quality areas matching well with our intuition.

To verify GraspPF quantitatively, a comparison experiment is conducted with baseline algorithms to predict grasp. We adopt GraspNet[3], Contact-GraspNet (abbreviated to Con-GraspNet)[11], and GG-CNN[1] as baselines because they are reported to show good performance in a cluttered environment, and the original implementations are provided. We use the pre-trained models provided by authors despite using a different gripper model (Robotis Hand) in our experiment than that of baselines. We reduce the impact by utilizing the predicted grasp width for pre-grasp motion, making the interaction between the gripper and other entities minimal. GG-CNN is executed in a closed-loop manner, whose implementation reproduces the original Kinova Mico experiment. Throughout all baselines, we filter out grasp candidates by $Q^{R}$. GraspPF is executed in two versions: GraspPF-ol, GraspPF-cl. GraspPF-ol is an open-loop version of GraspPF, which uses the particle filter as a refinement with a fixed observation. On the other hand, GraspPF-cl runs in a closed-loop manner and continuously refines grasps during approaching motion.

Additionally, we also implement five ablation versions of GraspPF: Sampling-ol, Sampling-cl, GraspPF-TD, GraspPF-real, and GraspPF-syn. Sampling-ol is a method that predicts grasps through forward sampling by DGQ-CNN and $Q^{R}$ (without refinement) from global observations at a fixed camera pose, where grasping is executed in an open-loop manner. Sampling-cl is a closed-loop version of Sampling-ol, where a batch of grasps is predicted every time step, and the grasp is selected by predicted quality and distance regularizer from the previously selected grasp. GraspPF-TD is the same as GraspPF-cl except for reducing grasp space to top-down (i.e., 4DoF). Lastly, in GraspPF-real, only real data is used to train DGQ-CNN, whereas GraspPF-syn uses only synthetic data.

The experiment protocol is as follow: 1) 62 unknown objects are placed in the box 2) pour them on the flat table to construct a heavily cluttered environment 3) set the initial gripper pose with a variety of distance to the table which is uniformly selected within 0.45-0.65m 4) execute the grasping algorithm and log the result 5) repeat 3-4) for 20 times and check the success rate. We repeat the experiment 7 times for each algorithm, and the total number of grasping trials is 140. The comparison results are reported in the Tab. II.

First, in terms of open-loop performance, the proposed algorithm GraspPF-ol outperforms baseline algorithms GraspNet and Con-GraspNet. Furthermore, the performance margin increases to 17% when applying closed-loop (GraspPF-cl). On the other hand, GraspPF-cl exceeds GGCNN-cl by 39%. We find that GGCNNs struggle to predict the quality grasp in a highly dense cluttered environment. Next, to study the individual influence of each factor, the results of 5 ablation experiments can be compared. Fixing approach direction to top-down (GraspPF-TD) degrade performance when compared with GraspPF-cl, and this result can be evidence that introducing 6DoF grasp space is beneficial, which is also studied in [9]. Next, to see how introducing a closed-loop affects performance, GraspPF-cl, Sampling-cl, GraspPF-ol, and Sampling-ol can be compared. Overall, applying closed-loop brings performance gain, which comes from improved robustness to control errors and sensor noise as previously studied in [15].

Regarding the influence of introducing particle filter, which means extracting more information from sequential observations, the success rate of Sampling-cl shows a 16% performance drop. This result implies that the system can keep the prior distribution better through particle filter and update the distribution with multi-view observations. On the other hand, the effect of introducing particle filter as a refinement step can be revealed by comparing Sampling-ol and GraspPF-ol, and it shows an improvement of 13%. This improvement can be explained by the fact that forward sampling is based on Monte-Carlo sampling, so it is not enough to cover a large continuous grasp space, although the space is reduced from pixel-wise prediction as explained in Sec. IV-C. Then particle filter can refine the initial grasps and find better grasps even with the same observation. Lastly, regarding the effects of dataset source, although performance declines in both GraspPF-real and GraspPF-syn, it degrades much more in GraspPF-real. A slump in GraspPF-real is because of the lack of variation of camera parameters, including intrinsic and viewpoint, while GraspPF needs more dynamic variations. The 9% drop in GraspPF-syn can be mainly from a sim-to-real gap in the depth sensor.

Furthermore, in the real cleaning-up experiment, the GraspPF-cl can chase an object, re-grasp after fail, and react to a sudden object change. These make the framework can clean all objects containing flat or deformable objects. The grasping failures mainly result from slippage. Specifically, the slippage arises while gripping because the estimated grasp region has a too obtuse angle, or the axial slippage results in the object’s fall during the placing because of not considering the center of gravity.