A Topological Solution of Entanglement for Complex-shaped Parts
in Robotic Bin-Picking

Topology coordinate is constructed between two tangled objects by three attributes [17], [18]. The first attribute is writhe, which explains how much the two curves are twisting around each other. For instance, entangled objects get a higher score than separated objects. Writhe between two objects is calculated by Gaussian Link Integral (GLI) as follows. If we have two curves $\gamma_{1}$ and $\gamma_{2}$ which are point sets in Cartesian Coordinate, writhe can be calculated by GLI as follows.

$$GLI({\gamma}_{1},{\gamma}_{2})=\frac{1}{4\pi}\int_{{\gamma}_{1}}\int_{{\gamma}_{2}}\frac{d{\gamma}_{1}\times{d{\gamma}_{2}}\cdot({\gamma}_{1}-{\gamma}_{2})}{||{\gamma}_{1}-{\gamma}_{2}||^{3}}$$

(1)

The second attribute is density, which describes how much the twisted area is concentrated on two curves. The third attribute is center, which is a location that explains the center location of the twisted area.

Given a single depth image of a cluttered scene, the topology coordinate can be constructed to measure the entanglement (Fig. 3). Writhe is a scalar attribute that indicates how much the objects are tangled together. A depth map containing tangled objects has higher writhe than the one with objects just overlapped together. Density is also a scalar attribute that indicates the distribution of the entanglement is evenly or intensively on the depth map. Center indicates the center position of entanglement on the depth map.

Original topology coordinates [17] can only be applied for two characters and assume the exact position of characters are known. Different from the definition and calculation, we construct topology coordinates only using a single depth image containing multiple objects so that the position of each object remains unknown. Instead of computing the relationship between two objects, we extract the line segments of edge from the depth map and calculate the topology coordinates using the relationship between each pair of line segments. Edge contains all the information we need to describe the shape and position of objects. Even though the line segments of edges may not indicate the complete contours of all parts, the topological relationship calculated by line segments can still reflect how and where the entanglement occurs.

We use a depth map $I$ to compute topology coordinates $G=(w,\bm{c},d)$. In order to calculate these three attributes, we need to generate a matrix called writhe matrix $T$ firstly. Taken $I$ as input, we detect the edges and transfer them to a collection of 3-dimensional vectors $L=(l_{1},l_{2},...,l_{n})$. Writhe matrix $T$ is a $n\times{n}$ matrix that stores GLI of each segment pair in $L$. Particularly, instead of using Eq.(1), GLI between two 3-dimensional line segments is computed using the algorithm proposed by Klenin and Langowski [23]. For instance, $T_{i,j}$ in the writhe matrix between i-th segment $l_{i}$ and j-th segment $l_{j}$ can be calculated by

$$T_{i,j}=GLI(l_{i},l_{j})$$

(2)

It can be seen that the writhe matrix $T$ is an upper-triangle-like matrix where half of the elements in $T$ are zero. We can compute the writhe $w$, density $d$ and center $\bm{c}$ using writhe matrix $T$. First, writhe $w$ is the sum of all values in $T$ divided by the number of line segments as follows.

$$w=\frac{1}{n}{\sum_{i=1}^{n}\sum_{j=1}^{n}{T_{i,j}}}$$

(3)

Then, density $d$ is calculated by the ratio of the pairs that have higher values in writhe matrix $T$. We extract the non-zero elements from $T$ and compute $d$ using the number of elements higher than some threshold divided by the total number of non-zero elements. Here, we define the threshold as the mean of extracted non-zero elements. Finally, center $\bm{c}$ is simply obtained by the center of mass for matrix $T$, which is a segment pair that contributes the most to the entanglement. Moreover, we introduce a mask called center mask which has the same size as the input depth image (Fig. 4). A center mask is a binary matrix with an area consisting of both center segments.

Let us explain how the topology coordinates service for the entanglement using a depth map.

Firstly, writhe is a quantitative measure that denotes how much the entanglement is on the depth image, while density and center denote the position information for the entanglement. Therefore, we present the visualization of the writhe matrix and the center mask to elaborate the density and center more intuitively. Fig. 4 presents which region is the entanglement center from the input depth image while Fig. 5 shows four different clutters with various writhe and density values by presenting the corresponding input depth images, detected edge segments, and visualized writhe matrices. This visualized matrix derives from $T$ in Section.III-C since it only remains the larger elements and is resized to a certain size. We can observe which pairs of edge segments share the larger writhe value and what is the distribution of the segment pairs from the matrix. If the edge segments are tangled with those near them, the brighter values are concentrated around the axis of the matrix. On the contrary, if the edge segments share rather larger writhe values with those all over the image, the distribution in the writhe matrix is rather even. Therefore, the more concentrated around the axis in the writhe matrix, the higher the corresponding density is. The details are elaborated as follows.

Writhe. Fig. 5(a) is the situation where two objects are twisting together while Fig. 5(b) refers to two simply overlapped objects. They have similar density $d$ but differ from writhe $w$. The writhe of twisted objects is larger than overlapped ones. If the robot wants to pick objects from these scenes, Fig. 5(b) with lower writhe has s higher possibility of a successful picking.

Density. From Fig. 5(c-d) we can tell by the human observation that Fig. 5(d) would be a better choice for robot simply by taking a look. Visualized writhe matrix and density value can also explain the scene numerically. The visualized matrix in Fig. 5(d) has an even distribution of brighter pixels since every line segment tends to tangle with more segments. Because as the number of segment pairs with larger GLI increases, the number of bright pixels in the visualized matrix also increases. Thus, these brighter pixels distribute more evenly, in other words, the density becomes smaller. On the contrary, every object in Fig. 5(c) is twisted with the other objects, thus, the entanglement is distributed intensely on the depth map. For the visualized matrix, the pixels with larger writhe are concentrated around the axis. Therefore, when writhe values are similar, density can also contribute to entanglement analysis.

Center. The center is computed by the center of mass of the writhe matrix, which is a pair of line segments that contributes the most to the entanglement. We present how the center affects the entanglement by presenting a mask that has the same size as the input depth image (Fig. 4). The center mask indicates the position information of the entanglement but not as much as writhe and density do.

To summarize, by focusing on the metrics of the entanglement regardless of the number of objects, situations with lower writhe and lower density is preferred. Therefore, the topology coordinate proposed in this section can be used to determine where a non-tangle grasp should be located.

We select a parallel jaw gripper and use a single depth map as input to compute grasp hypotheses. The overview of the proposed grasp detection method is illustrated in Fig. 7. First, a depth map is captured and used to construct the topology coordinate. Then, we use the writhe in topology coordinates to determine if the objects in the bin are tangled. If not, grasp is detected only considering the collision. If the entanglement exists, we calculate the entanglement map which evaluates where the potential tangled parts are in the box. We crop several regions with high possibilities of containing graspable objects from the entanglement map. Finally, grasp is detected and ranked in each selected region using graspability measure [2].

We explain how to compute an entanglement map by a given depth map $I$. First, we compute the line segments for edges on input depth map $I$, and generate topology coordinate $G=(w,\bm{c},d)$ along with center mask $C_{mask}$ for $I$ . Then, we use a pre-defined sliding window function for $I$ to obtain expanding information. For each window, we calculate its own writhe and density. This sliding window function returns two matrices $W_{s}$, $D_{s}$, which respectively store writhe and density of each region. The combination of two matrices refers to the rough entanglement information on each regions from $I$. However, we would like to precisely evaluate the entanglement situation upon the whole image. We use calculated topology coordinate $G$ to evaluate the weights for these matrices and center mask. The initial weights are manually defined as ${\sigma}_{W}=0.8$, ${\sigma}_{D}=0.15$, ${\sigma}_{D}=0.05$ respetively for $W_{s}$, $D_{s}$ and $C_{mask}$ since writhe affect more on predicting potential tangled regions. If $\overline{d}$, average of $D_{s}$, is larger than $d$ of the coordinate $G$, it means that density may affect the result of entanglement map generation. Therefore, the weights are modified as,

$${\sigma}_{D}^{*}=(\overline{d}/d)\sigma_{D};\quad{\sigma}_{W}^{*}=1-{\sigma}_{D}^{*}-{\sigma}_{C}$$

(4)

The center mask is independent of the sliding window algorithm so that the weight ${\sigma}_{C}$ remains the same. Finally, entanglement map $E$ is obtained by the addition of weighted metrics as Eq.(5) shows following a bi-linear interpolation.

$$E={{{\sigma}_{W}}^{*}}W_{s}+{{\sigma}_{D}^{*}}D_{s}+{{\sigma}_{c}}C_{mask}$$

(5)

Some examples are presented in Fig. 8. In our perspective, the entanglement map is the visualization that indicates possibilities of entanglement in every region for the whole depth map. We can observe those areas where objects are heavily tangled with each other are marked as yellow, while blue areas refer to non-tangled regions. We prefer to generate grasps on blue areas.

We present a grasp detection method considering both entanglement and collisions. Another sliding window function is executed to seek several region candidates with smaller values from the entanglement map. In each region, a grasping pose can be computed using graspability measure [2]. Graspability is an index for detecting a grasping point by convoluting a template of contact areas and collision areas for a robot hand. To put it more precisely, it is based on the idea that the object should be in the trajectory of hand closing, and there should be no object in the position to lower the robot hand. We use a parallel jaw gripper and rotate the gripper template along for 4 orientations. For the detected grasp candidates in each region, we rank them simply by pixel values in the entanglement map of the corresponding positions combined with the graspability score. Finally, the best grasp position is selected as the top of ranked grasp candidates.

We perform several real-world robot experiments to evaluate our method in bin-picking. We use NEXTAGE from Kawada Robotics and set a fixed 3D camera YCAM3D-II one meter straight above from the bin. We use Choreonoid and graspPlugin to simulate and execute the movement of the robot. The execution time was recorded on a PC running Ubuntu 16.04 with a 2.7 GHz Intel Core i5-6400 CPU. Our experiment system is set as Fig. 9 shows.

As Fig. 10 shows, two types of industrial parts with complex shapes are selected. We prepare three patterns of clutter state by only C-shaped objects, only S-shaped objects, and mixed objects. In particular, three picking trials are performed for each clutter. Each trial contains 20 times of picking to record the success rate of only picking one object.

The purpose of the experiment is to compare our method with two baselines.

1) Graspability. We select graspability measure [2] which is a general grasp detection algorithm only using a hand template. It outputs several grasp candidates ranked by graspability measure.

2) CNN. We also select previous work from [1] which is the first approach to predict potential tangled objects in bin-picking. It takes the same grasp candidates as Graspabilty, but ranks them with a prediction network.

First, we evaluate the success rates and time costs for bin-picking experiments (Table I). The number after slash denotes to the total picking times of one trial, and the one before the slash is the number of times when the robot picks up only one object. As a baseline, Graspability struggles in success rates since it can not discriminate whether the target is entangled with others or not. Our method and CNN both reach relatively higher success rates for picking a single object. Particularly, our method improves the performance of picking from S-shaped objects by a success rate of 50$\%$. The reason why CNN struggles with an S-shaped object is that it uses quarters of depth map to make predictions. Even if the cropped image contains the complete shape of target objects, it still lacks information of entangling with others. Our method directly evaluates entanglement for a complete depth map to solve the problem bothering CNN. For the mixed objects, our method reaches a high success rate of 70$\%$ since our model-free method only focuses on the information of edges in the depth map. The superior performance of our method indicates that our hand-engineered approach can analyze the relationship between these objects directly and efficiently. CNN may require more evaluation for generalization while our method can be utilized without training.

From the examples presented in Fig. 11, we validate how our method selects graspable objects qualitatively. For the same depth map as input, we use baselines and our method to detect optimal grasp positions, and the top-ranked grasp is marked using green dots. It is observed that our method can directly find the objects that are not tangled with others in the bin, while Graspability and CNN always focus on objects at the top of the clutter. Especially in the fifth column when all five grasp candidates are classified as tangled, both existing approaches predict poorly while our method successfully finds the graspable objects without any entanglement in the bin. Graspable objects selected by the proposed method are similar to the human observations. The reason is that our method uses edge and topological knowledge to explain the entanglement relationship more intuitively, which guarantees a complete observation of all potential entanglement in the bin.

In addition, the average time costs of Graspability, CNN, and our method are 2.1s, 2.7s, 7.8s. The time cost of our method depends on how many line segments are detected from the depth map. For our experiment setting of 10 objects placed in the bin, the time cost per trial is limited to 8s.

Let us consider other kinds of industrial parts like Fig. 12 shows. This type of object provides too much edge information for topology coordinates, which may cause some misunderstandings. In this case, a learning-based approach would be necessary. However, since our method prefers objects with the shape of pure edges such as rigid linear objects with a smooth edge, it is possible to develop our method on manipulation of deformable linear objects such as tubes or ropes in future work.

Common failures that result from our method are caused by the situation where the selected region does not contain any graspable positions. Even if the entanglement map can be generated correctly, grasps can not be detected due to the collisions in the selected region. In the future, it may be possible to add more collision information during generating the entanglement map to improve the performance.