Fast High-Quality Tabletop Rearrangement in Bounded Workspace

Consider a 2D bounded workspace $\mathcal{W}\subset\mathbb{R}^{2}$ containing a set of $n$ objects $\mathcal{O}=\{o_{1},...,o_{n}\}$. Each object is assumed to be an upright generalized cylinder. A feasible arrangement $\mathcal{A}=\{p_{1},...,p_{n}\}$, $p_{i}=\{x_{i},y_{i},\theta_{i}\}\in SE(2)$ is a set of poses for objects in $\mathcal{O}$, such that (1) each object’s footprint is contained in $\mathcal{W}$, and (2) no two objects collide.

We consider (overhand) pick-n-place actions to move objects one by one. A pick-n-place action, represented as an ordered pair $(o,p)$, grasps an object $o$ at its current pose and lifts it above all other objects. It then moves it horizontally, and places it at the target pose $p$ within $\mathcal{W}$. An action is collision-free if and only if both the current and resulting arrangements are feasible. A plan $P$ from a feasible $\mathcal{A}_{s}$ to a feasible $\mathcal{A}_{g}$ is a sequence of collision-free pick-n-place actions transforming $\mathcal{A}_{s}$ into $\mathcal{A}_{g}$. We want to compute feasible plans that minimize the number of pick-n-place actions, which leads to increased system throughput. In summary:

It is not always possible to move an object to its goal pose, which may be occupied by other objects. This leads to dependencies between objects, i.e., when the goal pose $p^{g}_{i}$ of $o_{i}$ collides with $o_{j}$ at its current pose, then $o_{i}$ depends on $o_{j}$ and $o_{j}$ must be moved before moving $o_{i}$ to its goal.

Dependencies induced by $\mathcal{A}_{s}$ and $\mathcal{A}_{g}$ give rise to a dependency graph $G$ [18, 1]. Fig.  2 provides an example graph. When $G$ is acyclic, the instance is called ”monotone” and can be solved with at most $n$ actions moving each object once from its start to its goal pose following the topological order of $G$. Otherwise, some object(s) must be temporarily displaced to break these cycles and complete the task. We refer to these intermediate poses as ”buffers”, which may be external (i.e., outside $\mathcal{W}$) or internal (i.e, contained in $\mathcal{W}$). If the buffers must be internal, the problem is TORI. Otherwise, the problem is TORO with external buffers, denoted as TORE. Solving TORE only requires dealing with inherent constraints defined by $G$. For instance, to solve the problem in Fig. 2, we can move the Pepsi to an external buffer to break the cycle, move the Coke first and then Fanta to their goal locations and finally bring back the Pepsi into the workspace. With internal buffers, we must find a temporary location for the Pepsi in $\mathcal{W}$. Due to acquired constraints (as defined in Sec. I) arising from internal buffer selection, TORI, the problem we study in this work, is more challenging than TORE. Intuitively, selecting buffers inside workspace (TORI) is much more difficult and constrained than using buffers outside the workspace (TORE) to store displaced objects.

Nevertheless, we show here that plans can be efficiently derived from the minimum total buffer solution to TORE [1] and the minimum running buffer solution to TORE [2], which computes the minimum number of concurrent external buffers needed to solve a TORE instance. For both objectives, since TORE has been shown to be computationally intractable [1, 2] and is a special case of TORI, TORI is also NP-hard.

When an object stays at a buffer, it should avoid blocking the upcoming manipulation actions of other objects. Otherwise, either the object in the buffer or the manipulating object has to yield, which increases the number of necessary actions. In other words, we need to carefully choose acquired constraints. If we know the schedule of other objects in advance, a buffer can be selected to minimize unnecessary obstructions. This observation motivates solving the rearrangement problem in two steps: First, compute a primitive plan, which is an incomplete schedule ignoring acquired constraints; second, given the incomplete schedule as a reference, generate buffers to optimize the selection of acquired constraints.

The TRLB framework builds on the insight that a new TORI instance is generated when lazy buffer allocation fails. The new instance has the same goal $\mathcal{A}_{g}$ as the original one but some progress has been made in solving the TORI task. There are two straightforward implementations of TRLB: forward search and bidirectional search. In the first case, by accepting partial solutions, a rearrangement plan can be computed by developing a search tree $T$ rooted at $\mathcal{A}_{s}$. In the search tree $T$, nodes are feasible arrangements and edges are partial plans containing a sequence of collision-free actions. When buffer allocation fails, we add the resulting arrangement into the tree and resume the rearrangement task from a random node in $T$. This randomness and the randomness in primitive plan computation and buffer allocation allows TRLB to recover from failures.

In bidirectional search, two search trees rooted at $\mathcal{A}_{s}$ and $\mathcal{A}_{g}$ are developed. This more involved procedure is shown in Algo. 2, which computes two search trees that connect $\mathcal{A}_{s}$ and $\mathcal{A}_{g}$. In line 1, the trees are initialized. For each iteration, we first rearrange between a random node $\mathcal{A}_{rand}$ on $T_{1}$ to the root node of $T_{2}$ (line 3-5). The function LazyBufferAllocation refers to the overall algorithm developed in Sec. IV-A. A found path yields a feasible plan for TORI (line 6). Otherwise, we rearrange between the new arrangement $\mathcal{A}_{new1}$ and its nearest neighbor in $T_{2}$ (line 7-9). If a path is found, then we find a feasible rearrangement plan for TORI (line 10). Otherwise, we switch the trees and attempt rearrangement from the opposite side (line 11).

In dense environments, allocating buffers is hard, motivating minimizing the number of running buffers [2], which is generally low even in high density settings if we treat objects as unlabeled. Based on this, for each component of the dependency graph that is not a single vertex/cycle, we reduce the running buffer size to at most 1 by first solving an unlabeled instance [2]. After preprocessing, we obtain a TORI requiring at most one running buffer. Fig. 4 shows an example of preprocessing. $o_{1}$, $o_{2}$ and $o_{3}$ form a complete graph, where at least two objects need to be placed at buffers simultaneously. We conduct preprocessing of the three-vertex component by moving $o_{2}$ to a buffer position, $o_{1}$ to $p^{g}_{3}$ and $o_{3}$ to $p^{g}_{2}$. $o_{2}$ will not move to $p^{g}_{1}$ since it does not occupy other goal poses. The preprocessing step needs one buffer and the resulting rearrangement problem is monotone.

We first present experiments with cylindrical objects to compare lazy buffer generation algorithms given different options, including: (1) Primitive plan computation: running buffer minimization (RBM), total buffer minimization (TBM), random order (RO); (2) Buffer allocation methods: optimization (OPT), sampling (SP); (3) High level planners: one-shot (OS), forward search tree (ST), bidirectional search tree (BST); and (4) With or without preprocessing (PP). Here, the one-shot (OS) planner is using primitive plans and buffer allocation (Sec. IV-A) without tree search (Sec. IV-B). In OS, we attempt to compute a feasible rearrangement plan up to $30|\mathcal{O}|$ times before announcing a failure. Notice that at least $|\mathcal{O}|$ actions are required for solving any instance.

A full TRLB algorithm is a combination of components, e.g., RBM-SP-BST stands for using the primitive plans that minimize running buffer size, performing buffer allocation by sampling, maintaining a bidirectional search tree, and doing so without preprocessing.

For evaluation, we first compare the primitive plan computation options, using sampling-based buffer allocation, bidirectional tree search and no preprocessing. TBM and RBM plans are computed using dynamic programming based solvers [2]. The results are shown in Fig.6. Even though plans generated by TBM-SP-BST are slightly shorter than RBM-SP-BST, TBM-SP-BST is less scalable as either the density level or the number of objects in the workspace increases. Compared to RBM plans, individual RO plans can be generated almost instantaneously but we don’t see much benefit in computation time for the overall algorithm. The results indicate that RBM should be used for primitive plan computation as it results in efficient and high-quality solutions.

In Fig. 7, buffer allocation methods are compared using the RBM primitive planner and the OS high-level planner. Optimization-based allocation guarantees completeness and generates high-quality plans but it is computationally expensive. When $\rho=0.5$, the success rate tends to be low in instances with a small number of objects. That is because for the given density level, the smaller the number of objects, the larger the object size relative to the environment, and the smaller the configuration space size relative to the environment. Thus, precisely allocating buffer locations with OPT is helpful in these cases.

The effectiveness of the high-level planners and preprocessing are shown in Fig. 8, which suggests that ST, BST and preprocessing are all effective in increasing success rate in dense environments. In addition, preprocessing significantly speeds up computation in large scale dense cases at the price of extra actions to execute preprocessing. By simplifying the dependency graph with preprocessing, less time is needed to compute a primitive plan.

The robustness of ST and BST are further evaluated with “dense-small” instances where a few objects are packed densely (Fig. 9). The bidirectional search tree has a higher success rate in these cases, especially in 5-object instances.

In addition to the above evaluations, we also tried integrating the preprocessing into the BST framework (RBM-SP-BST-PP), which speeds up the computation: for $60$-object instances with $\rho=0.5$, only $63\%$ of them can be solved by RBM-SP-BST in 300 seconds. All of them, however, can be solved together with the preprocessing and the solution time averaged 0.29 seconds. Similarly to the results in Fig. 8, preprocessing makes the solution plan much longer than necessary (needs around $30\%$ more actions than RBM-SP-BST). Based on the analysis on computation time, success rate, and solution quality, RBM-SP-BST is the best overall combination, and preprocessing significantly speeds up the solver with a reduction of solution quality.

We compare the proposed method RBM-SP-BST with BiRRT(fmRS) [14] and a MCTS planner [15], which, to the best of our knowledge, are state-of-the-art planners for TORI. The MCTS planner is a C++ solver, while the other two methods are implemented in Python. Besides success rate, solution quality, and computation time, we also compare the number of collision checks which are time-consuming in most planning tasks. In Fig. 10, we compare the methods in large scale problems with $\rho=0.3$. The success rate is $100\%$ for all. Our method, RBM-SP-BST, avoids repeated collision checks due to the use of the dependency graph. BiRRT(fmRS), which only uses dependency graphs locally, spends a lot of time and conducts a lot of collision checks to generate random arrangements. MCTS generates solutions with similar optimality but does so also with a lot of collision checking, which slows down computation. We note that a value of $1$ in the right figure (number of actions) is the minimum possible, so both RBM-SP-BST and MCTS compute high-quality solutions, which RBM-SP-BST does slightly better. To sum up, in sparse large scale instances, RBM-SP-BST is two magnitudes faster and conducts much fewer collision checks than the alternatives.

Next, we compare the methods in “dense-small” instances (Fig. 11). Here, RBM-SP-BST is the only method that maintains high success rate in these difficult cases.

We further compare the performance of RBM-SP-BST and MCTS in lattice rearrangement problems, which are recently studied in the literature [27]. An example with 15 objects is shown in Fig. 12[left]. In the start and goal arrangements, gaps between adjacent objects are set to be 0.01 object radius, and thus buffer allocation is challenging for sampling-based methods. While MCTS tries all the actions on each node, RBM-SP-BST is able to detect the embedded combinatorial object relationship via the dependency graph and therefore needs less buffer allocation calls. As shown in Fig. 12[right], RBM-SP-BST has much higher success rate in lattice rearrangement tasks.

Because the MCTS solver only supports cylindrical objects, we only compare RBM-SP-BST and BiRRT(fmRS) in the cuboids setup (Fig. 5[right]). When $\rho=0.3$, RBM-SP-BST computes high quality solutions efficiently, while BiRRT(fmRS) can only solve instances with up to 20 cuboids. We mention that, when $\rho=0.4$, BiRRT(fmRS) cannot solve any instance, but RBM-SP-BST can solve 50-object rearrangement problems in 28.6 secs on average.

We further demonstrate that the plans computed by TRLB can be readily executed on real robots in a complete vision-planning-control pipeline. Our hardware setup consists of a UR-5e robot arm, an OnRobot VGC 10 vacuum gripper, and an Intel RealSense D435 RGB-D camera. As shown in the accompanying video, TRLB solves all attempted instances (Fig 1 and Fig. 14), which involves concave objects, in an apparently natural and efficient manner.