# Detecting and Solving Tube Entanglement in Bin Picking Operations
## 3 Bin Picking Algorithm
### Trajectory Planning
#### 3.3.1 Trajectory Synthesis  
Two sorts of trajectories are considered: an _upwards trajectory_ (where only the z component of the gripper's position is affected) and an _escape trajectory_. This second trajectory begins by performing a small upwards motion, followed by a movement along a line on the \(x\) and \(y\) axes to try to move the tube so that it is no longer occluded, and ends with another larger upwards trajectory.  
If the set of non-occluded tubes is not empty, then an upwards trajectory is created for each of them with a cost of 0, and the algorithm moves on to the grasp planning phase directly. Therefore, the simulator is not used, as it is assumed that since these tubes do not have any other object on top of them, as long as a suitable grasping point is chosen, lifting them should not lead to a situation of entanglement. Avoiding using the simulator for these simpler cases has the advantage of considerably shortening the algorithm's processing time.  
If no suitable grasps are found for any of the non-occluded tubes, then the weakly-occluded tubes are considered. For each of these tubes, three trajectories are generated: the upwards trajectory and two escape trajectories that try to free the tube from the occluding object by performing a lateral movement in both directions of the escape line, defined below.  
Definition 13: The **escape line** of an occluded tube is the orthogonal projection of the line that passes through both of the occluded joint's endpoints on the xOy plane.  
The distance \(d_{escape}\) along the escape line that the gripper moves in one of the directions is given by Equation (1), which is a sum of a variable term, \(d_{var}\), and a constant term, \(d_{const}\).  
\[d_{escape}=d_{var}+d_{const} \tag{1}\]Let \(occl\) be one of the two endpoints of the occluded joint, \(EP(occl)\) the set of cylinder endpoints on the same section as \(occl\), and \(proj(P)\) the projection of point \(P\) on the escape line. The variable term (Equation (2)) is the distance between the occlusion and the farthest endpoint projected on the escape line. It varies according to which direction the tube is moved.  
\[d_{var}=max(\{d\mid d=Euclidean\_distance(proj(occl),proj(ep)),ep\in EP( occl)\}) \tag{2}\]  
The constant term is a predefined distance that significantly increases the chances of solving the entanglement by accounting for possible inaccuracies during the tube modeling. It should be at least as large as three times the tube's radius. This is due to the possibility of the length of an occlusion being as large as the occluding tube's diameter. In addition, the farthest endpoint may not be the farthest point of the section to the occlusion, which occurs when the axis of the cylinder containing the farthest endpoint is not parallel to the escape line.  
Figure 3 provides a visual aid for the escape line and distances.  
To prevent the tube from colliding with the bin's walls, it is checked if the bounding box of the virtual tube (in the \(x\) and \(y\) axes) after applying the horizontal translation of the escape trajectory intersects a predefined rectangular safety zone, which is smaller than the bin's boundaries. If this intersection occurs, then the escape distance is shortened to be the largest distance along the escape line for which such a collision does not occur.