1

Complexity Results and Fast Methods for OptimalTabletop Rearrangement with Overhand Grasps

Shuai D Han, Nicholas M Stiffler, Athanasios Krontiris, Kostas E Bekris, and Jingjin Yu

Abstract—This paper studies the underlying combinatorialstructure of a class of object rearrangement problems, whichappear frequently in applications. The problems involve multiple,similar-geometry objects placed on a flat, horizontal surface,where a robot can approach them from above and perform pick-and-place operations to rearrange them. The paper considersboth the case where the start and goal object poses overlap,and where they do not. For overlapping poses, the primaryobjective is to minimize the number of pick-and-place actionsand then to minimize the distance traveled by the end-effector.For the non-overlapping case, the objective is solely to minimizethe travel distance of the end-effector. While such problems donot involve all the complexities of general rearrangement, theyremain computationally hard in both cases. This is shown throughreductions from well-understood, hard combinatorial challengesto these rearrangement problems. The reductions are also shownto hold in the reverse direction, which enables the convenientapplication on rearrangement of well studied algorithms. Thesealgorithms can be very efficient in practice despite the hardnessresults. The paper builds on these reduction results to proposean algorithmic pipeline for dealing with the rearrangementproblems. Experimental evaluation, including hardware-basedtrials, shows that the proposed pipeline computes high-qualitypaths with regards to the optimization objectives. Furthermore,it exhibits highly desirable scalability as the number of objectsincreases in both the overlapping and non-overlapping setup.

I. INTRODUCTION

In many industrial and logistics applications, such as thoseshown in Fig. 1, a robot is tasked to rearrange multiple, similarobjects placed on a tabletop into a desired arrangement. Inthese setups, the robot needs to approach the objects fromabove and perform a pick-and-place action at desired targetposes. Such operations are frequently part of product packag-ing and inspection processes. Efficiency plays a critical rolein these domains, as the speed of task completion has a directimpact on financial viability; even a marginal improvement inproductivity could provide significant competitive advantagein practice. Beyond industrial robotics, a home assistant robotmay need to deal with such problems as part of a roomcleaning task. The reception of such a robot by people will bemore positive if its solutions are efficient and the robot doesnot waste time performing redundant actions. Many subtasksaffect the efficiency of the overall solution in all of theseapplications, ranging from perception to the robot’s speed ingrasping and transferring objects. But overall efficiency alsocritically depends on the underlying combinatorial aspects ofthe problem, which relate to the number of pick-and-placeactions that the robot performs, the placement of the objects,as well as the sequence of objects transferred.

This paper deals with the combinatorial aspects of tabletop

Fig. 1. Examples of robots deployed in industrial settings tasked to arrangeobjects in desired configurations through pick and place: (left) ABB’s IRB360 FlexPicker rearranging pancakes (right) Staubli’s TP80 Fast Picker robot.

object rearrangement tasks. The objective is to understand theunderlying structure and obtain high-quality solutions in acomputationally efficient manner. The focus is on a subsetof general rearrangement problems, which relate to the abovementioned applications. In particular, the setup corresponds torearranging multiple, similar-geometry, non-stacked objects ona flat, horizontal surface from given initial to target arrange-ments. The robot can approach the objects from above, pickthem up and raise them. At that point, it can move them freelywithout collisions with other objects.

There are two important variations of this problem. Thefirst requires that the target object poses do not overlap withthe initial ones. In this scenario, the number of pick-and-placeactions is equal to the number of objects not in their goal pose.Thus, the solution quality is dependent upon the sequencewith which the objects are transferred. A good sequence canminimize the distance that the robot’s end-effector travels. Thesecond variant of the problem allows for target poses to overlapwith the initial poses, as in Fig. 2a-c. The situation sometimesnecessitates the identification of intermediate poses for someobjects to complete the task. In such cases, the quality of thesolution tends to be dominated by the number of intermediateposes needed to solve the problem, which correlates to thenumber of pick-and-place actions the robot must carry out.The primary objective is to find a solution, which uses theminimum number of intermediate poses and among themminimize the distance the robot’s end-effector travels.

Both variations include some assumptions that simplifythese instances relative to the general rearrangement problem.The non-overlapping case in particular seems to be quite easysince a random feasible solution can be trivially acquired.Nevertheless, this paper shows that even in this simpler setup,the optimal variant of the problem remains computationallyhard. This is achieved by reducing the Euclidean-TSP prob-lem [1] to the cost-optimal, non-overlapping tabletop objectrearrangement problem. Even in the unlabeled case, where

arX

iv:1

711.

0736

9v1

[cs

.RO

] 1

7 N

ov 2

017

2

objects can occupy any target pose, the problem is still hard.For overlapping initial and final poses, the paper employs agraphical representation from the literature [2], which leads tothe result that finding the minimum number of pick-and-placeactions relates to a well-known problem in the algorithmiccommunity, the “Feedback Vertex Set” (FVS) problem [3].This again indicates the hardness of the challenge.

The benefit of these two-way reductions, beyond the hard-ness results themselves, is that they suggest algorithmicsolutions and provide an expectation on the practical effi-ciency of the methods. In particular, Euclidean-TSP admitsa polynomial-time approximation scheme (PTAS) and goodheuristics, which implies very good practical solutions for thenon-overlapping case. On the other hand, the FVS problem isAPX-hard [3], [5], which indicates that efficient algorithms areharder for the overlapping case. This motivated the consider-ation of alternative heuristics for solving such challenges thatmake sense in the context of object rearrangement.

The algorithms proposed here, which arose by mappingthe object rearrangement variations to well-studied problemshave been evaluated in terms of practical performance. Forthe non-overlapping case, an alternative solver exists that wasdeveloped for a related challenge [6]. The TSP solvers achievesuperior performance relative to this alternative when appliedto object rearrangement. They achieve sub-second solutiontimes for hundreds of objects. Optimal solutions are shownto be significantly better than the average, random, feasiblesolution. For the overlapping case, exact and heuristic solversare considered. The paper shows that practically efficientmethods achieve sub-second solution times without a majorimpact in solution quality for tens of objects.

This article expands an earlier version of this work [7]and includes the following new contributions: (i) a proofshowing that cost-optimal unlabelled non-overlapping table-top rearrangement is NP-hard, (ii) a complete descriptionof ILP-based heuristics and an algorithm for solving theobject rearrangement problem with overlap sub-optimally, (iii)significantly enhanced evaluation with experiments conductedon a hardware platform (Fig. 2d), corroborating the real-worldbenefits of the proposed method.

The structure of this manuscript is as follows. Section IIreviews related work, followed by a formal problem statementin Section III. Section IV considers the object rearrangementproblem when the objects have non-overlapping start and goalarrangements. The more computationally challenging problemthat involves overlapping start and goal arrangements appearsin Section V. The evaluation of the proposed methods is brokendown into two components: simulation (Section VI) and phys-ical experiments (Section VII). The paper concludes with asummary and remarks about future directions in Section VIII.

II. CONTRIBUTION RELATIVE TO PRIOR WORK

Multi-body planning is a related challenge that is itselfhard. In the general, continuous case, complete approachesdo not scale even though methods exist that try to decreasethe effective DOFs [8]. For specific geometric setups, suchas unlabeled unit-discs among polygonal obstacles, optimality

can be achieved [9], even though the unlabeled case is still hard[10]. Given the hardness of multi-robot planning, decoupledmethods, such as priority-based schemes [11] or velocity tun-ing [12], trade completeness for efficiency. Assembly planning[13], [14], [15] deals with similar problems but few optimalityarguments have been made.

Recent progress has been achieved for the discrete variantof the problem, where robots occupy vertices and movealong edges of a graph. For this problem, also known as“pebble motion on a graph” [16], [17], [18], [19], feasibilitycan be answered in linear time and paths can be acquiredin polynomial time. The optimal variation is still hard butrecent optimal solvers with good practical efficiency havebeen developed either by extending heuristic search to themulti-robot case [20], [21], or utilizing solvers for other hardproblems, such as network-flow [22], [23]. The current work ismotivated by this progress and aims to show that for certainuseful rearrangement setups it is possible to come up withpractically efficient algorithms through an understanding ofthe problem’s structure.

Navigation among Movable Obstacles (NAMO) is a relatedcomputationally hard problem [24], [25], [26], [27], where arobot moves and pushes objects. A probabilistically completesolution exists for this problem [28]. NAMO can be extendedto manipulation among movable obstacles (MAMO) [29] andrearrangement planning [30], [31]. Monotone instances forsuch problems, where each obstacle may be moved at mostonce, are easier [29]. Recent work has focused on “non-monotone” instances [32], [33], [34], [35], [36], [37]. Re-arrangement with overlaps considered in the current paperincludes “non-monotone” instances although other aspects ofthe problem are relaxed. In all these efforts, the focus ison feasibility and no solution quality arguments have beenprovided. Asymptotic optimality has been achieved for therelated “minimum constraint removal” path problem [38],which, however, does not consider negative object interactions.

The Pickup and Delivery Problem (PDP) [39], [40] is awell-studied problem in operations research that is similar totabletop object rearrangement, as long as the object geometriesare ignored. The PDP models the pickup and delivery of goodsbetween different parties and can be viewed as a subclassof vehicle routing [41] or dispatching [42]. It is frequentlyspecified over a graph embedded in the 2D plane, where asubset of the vertices are pickup and delivery locations. A PDPin which pickup and delivery sites are not uniquely pairedis also known as the NP-hard swap problem [43], [44], forwhich a 2.5-optimal heuristic is known [44]. Many exact linearprogramming algorithms and approximations are available[45], [46], [47] when pickup and delivery locations overlap,where pickup must happen some time after delivery. Thestacker crane problem (SCP) [48], [6] is a variation of PDPof particular relevance as it maps to the non-overlapping caseof labeled object rearrangement. An asymptotically optimalsolution for SCP [6] is used as a comparison point in theevaluation section.

This work does not deal with other aspects of rearrange-ment, such as arm motion [49], [50], [51], [52] or graspplanning [53], [54]. Non-prehensile actions, such as pushing,

3

(a) (b) (c) (d)Fig. 2. (a, b, c) An example of an object rearrangement challenge considered in this work from a V-REP simulation [4], where the initial (b) and final (c)object poses are overlapping and an object needs to be placed at an intermediate location. (d) In addition to the V-REP simulations, the solutions presentedin this work have been tested on an experimental hardware platform.

are also not considered [55], [56]. Similar combinatorialissues to the ones studied here are also studied by integratedtask and motion planners, for most of which there are nooptimality guarantees [57], [58], [33], [34], [59], [60]. Recentwork on asymptotically optimal task planning is at this pointprohibitively expensive for practical use [61].

III. PROBLEM STATEMENT

This section formally defines the considered challenges.

A. Tabletop Object Rearrangement with Overhand Grasps

Consider a workspace W with static obstacles and a setof n movable objects O = {o1, . . . , on}. For oi ∈ O, Cidenotes its configuration space. Then, Fi ⊆ Ci is the set ofcollision-free configurations of oi with respect to the staticobstacles in W . An arrangement R = {r1, . . . , rn} for theobjects O specifies the configurations ri ∈ Ci for each objectoi. A feasible arrangement is one satisfying:

1) ∀ ri ∈ R, ri ∈ Fi;2) ∀ ri, rj ∈ R, if i 6= j, then objects oi and oj are not in

collision when placed at ri and rj , respectively.This work focuses on closed and bounded planar

workspaces: W ⊂ R2. The setting is frequently referred toas the tabletop setup, in which the vertical projections of theobjects on the tabletop do not intersect. This work assumesthat the manipulator is able to employ overhand grasps,where an object can be transferred after being lifted aboveall other objects. In particular, a pick-and-place operation ofthe manipulator involves four steps:

a. bringing the end-effector above the object,b. grasping and lifting the object,c. transfer of the grasped object horizontally to its target

(horizontal) location, andd. a downward motion prior to releasing the object.

This sequence constitutes a manipulation action.The manipulator is initially at a rest position sM prior to

executing any pick-and-place actions and transitions to a restposition gM at the conclusion of the rearrangement task. Arest position is a safe arm configuration, where there is nocollision with objects.

The illustrations that appear throughout the paper assumeobjects with identical geometry. Nevertheless, the results de-

rived in this paper are not dependent on this assumption, i.e.,objects need only be general cylinders.1

Given the setup, the problem studied in the paper can besummarized as:Problem 1. Tabletop Object Rearrangement with Overhandgrasps (TORO). Given feasible start and goal arrangementsRS , RG for objects O = {o1, . . . , on} on a tabletop, deter-mine a sequence of collision-free pick-and-place actions withoverhand grasps A = (a1, a2, . . . ) that transfer O from RS

to RG.A rearrangement problem is said to be labeled if objects

are unique and not interchangeable. Otherwise, the problemis unlabeled. If for two arbitrary arrangements s ∈ RS andg ∈ RG, the objects placed in s and g are not in collision,then the problem is said to have no overlaps. Otherwise, theproblem is said to have overlaps.

This paper primarily focuses on the labeled TORO case andidentifies an important subcase:• TORO with NO overlaps (TORO-NO)

Remark 1. The partition of Problem 1 into the generalTORO case and the subcase of TORO-NO is not arbitrary.TORO is structurally richer and harder from a computationalperspective. Both versions of the problem can be extended tothe unlabeled and partially labeled variants. This paper doesnot treat the labeled and unlabeled variants as separate casesbut will briefly discuss differences that arise due to formulationwhen appropriate.

B. Optimization Criteria

Recall that a manipulation action ai has four components:an initial move, a grasp, a transport phase, and a release.Since grasping is frequently the source of difficulty in objectmanipulation tasks, it is assumed in the paper that graspsand subsequent releases induce the most cost in manipulationactions. The other source of cost can be attributed to the lengthof the manipulator’s path. This part of the cost is capturedthrough the Euclidean distance traveled by the end effectorbetween grasps and releases. For a manipulation action ai,the incurred cost is

cai = cmdie + cg + cmd

il + cr, (1)

1From differential geometry, a cylinder is defined as any ruled surfacespanned by a one-parameter family of parallel lines.

4

where cm, cg, cr are costs associated with moving themanipulator, a single grasp, and a single release, respectively.die and dil are the straight line distances traveled by the endeffector in the first (object-free) and third (carrying an object)stages of a manipulation action, respectively.

The total cost associated with solving a TORO instance isthen captured by

cT =

|A|∑i=1

cai = |A|(cg + cr) + cm

( |A|∑i=1

(die + dil) + df

), (2)

where df is the distance between the location of the last releaseof the end effector and its rest position gM . Of the two additiveterms in (2), note that the first term dominates the second.Because the absolute value of cg, cr, and cm are different fordifferent systems, the assignment of their absolute values isleft to practitioners. The focus of this paper is the analysisand minimization of the two additive terms in (2).

C. Object Buffer Locations

The resolution of TORO (Section V) may require the tem-porary placement of some object(s) at intermediate locationsoutside those in RS ∪ RG. When this occurs, external bufferlocations may be used as temporary locations for objectplacement. More formally, there exists a set of configurationsB = {b1, b2, . . . }, called buffers, which are available to themanipulator and do not overlap with object placements in RS

or RG.Remark 2. This work, which focuses on the combinatorialaspects of multi-object manipulation and rearrangement, uti-lizes exclusively buffers that are not on the tabletop. It isunderstood that the number of external buffers may be reducedby attempting to first search for potential buffers within thetabletop. Nevertheless, there are scenarios where the use ofexternal buffers may be necessary.

IV. TORO WITH NO OVERLAPS (TORO-NO)

When there is no overlap between any pair of start andgoal configurations, an object can be transferred directlyfrom its start configuration to its goal configuration. A directimplication is that an optimal sequence of manipulation actionscontains exactly |A| = |O| = n grasps and the same numberof releases. Note that a minimum of n grasps and releases arenecessary. This also implies that no buffer is required sinceusing buffers will incur additional grasp and release costs.Therefore, for TORO-NO, (2) becomes

cT = n(cg + cr) + cm

( n∑i=1

(die + dil) + df

), (3)

i.e., only the distance traveled by the end effector affects thecost. The problem instance that minimizes (3) is referred toas Cost-optimal TORO-NO. The following theorem provides ahardness result for Cost-optimal TORO-NO.Theorem IV.1. Cost-optimal TORO-NO is NP-hard.

Proof: Reduction from Euclidean-TSP [1]. Letp0, p1, . . . , pn be an arbitrary set of n + 1 points in 2D. Theset of points induces an Euclidean-TSP. Let dij denote the

Euclidean distance between pi and pj for 0 ≤ i, j ≤ n. In theformulation given in [1], it is assumed that dij are integers,which is equivalent to assuming the distances are rationalnumbers. To reduce the stated TSP problem to a cost-optimalTORO-NO problem, pick some positive ε � 1/(4n). Letp0 be the rest position of the manipulator in an objectrearrangement problem. For each pi, 1 ≤ i ≤ n, split piinto a pair of start and goal configurations (si, gi) such that(i) pi = si+gi

2 , (ii) si2 = gi2, and (iii) si1 + ε = gi1. Anillustration of the reduction is provided in Fig. 3. The reducedTORO-NO instance is fully defined by p0, RS = {s1, . . . , sn}and RG = {g1, . . . , gn}. A cost-optimal (as defined by (3))solution to this TORO-NO problem induces a (closed) pathstarting from p0, going through each si and gi exactly once,and ending at p0. Moreover, each gi is visited immediatelyafter the corresponding si is visited. Based on this path, themanipulator moves to a start location to pick up an object,drop the object at the corresponding goal configuration, andthen move to the next object until all objects are rearranged.Denote the loop path as P and let its total length be D.

p0

p1

p2

p0

s1 g1

s2 g2ε

ε

(a) (b)

Fig. 3. Reduction from Euclidean-TSP to cost-optimal TORO-NO

Assume that the Euclidean-TSP has an optimal solution pathPopt with a total distance of Dopt (an integer). Then P fromsolving the cost-optimal TORO-NO yields such an optimal pathfor the TSP. To show this, from P , simply contract the edgessigi for all 1 ≤ i ≤ n. This clearly yields a solution to theEuclidean-TSP; let the resulting path be P ′ with total lengthD′. As edges are contracted along P , by the triangle inequality,D′ ≤ D. It remains to show that D′ = Dopt. Suppose this isnot the case, then D′ ≥ Dopt+1. However, if this is the case,a solution to the TORO-NO can be constructed by splittingpi into si and gi along Popt. It is straightforward to establishthat the total distance of this TORO-NO path is bounded byDopt + nε < Dopt + n ∗ 1/(4n) = Dopt + 1/4 < Dopt + 1 ≤D′ ≤ D. Since this is a contradiction, D′ = Dopt.Remark 3. Note that an NP-hardness proof of a similarproblem can be found in [62], as is mentioned in [6].Nevertheless, the problem is stated for a tree and is non-Euclidean. Furthermore, it is straightforward to show that thedecision version of the cost-optimal TORO-NO problem is NP-complete; the detail, which is non-essential to the focus of thecurrent paper, is omitted.Remark 4. Interestingly, TORO-NO may also be reduced toa variant of TSP with very little overhead. Because highlyefficient TSP solvers are available, the reduction route pro-vides an effective approach for solving TORO-NO. That is,an TORO-NO instance may be reduced to TSP and solved,with the TSP solution readily translated back to a solution

5

to the TORO-NO instance that is cost-optimal. This is notalways a feature of NP-hardness reductions. The straightfor-ward algorithm for the computation is outlined in Alg. 1.The inputs to the algorithm are the rest positions of themanipulator and the start and goal configurations of objects.The output is the solution for TORO-NO, represented as asequence of manipulation actions A, which has completenessand optimality guarantees.

Algorithm 1: TORONOTSPInput: Configurations sM , gM , Arrangements RS , RG.Output: A sequence of manipulation actions A.

1 GNO ←CONSTRUCTTSPGRAPH(RS , RG, sM , sG)2 Sraw ← SOLVETSP(GNO)3 A ← RETRIEVEACTIONS(Sraw)4 return A

At Line 1 of Alg. 1, a graph GNO(VNO, ENO) is generatedas the input to the TSP problem. The graph is constructedfrom the TORO-NO instance as follows. A vertex is createdfor each element of RS and RG. Then, a complete bipartitegraph is created between these two sets of vertices. A set ofvertices U = {u1, . . . , u|RS |} is then inserted into edges sigifor 1 ≤ i ≤ |RS |. Afterward, sM (resp., gM ) is added as avertex and is connected to si (resp., gi) for 1 ≤ i ≤ |RS |.Finally, a vertex u0 is added and connected to both sM andgM . See Fig. 4 for the straightforward example for |RS | = 2.

sM

s1

s2

u1

u2

g1

g2 gM

u0

Fig. 4. An example of GNO for 2 objects. The nodes sM and gM denotethe initial and final rest positions of the manipulator end effector.

Let w(a, b) denote the weight of an edge (a, b) ∈ ENO.For all 1 ≤ i, j ≤ n, i 6= j (dist(x, y) denotes the Euclideandistance between x and y in 2D):

w(sM , u0) = w(gM , u0) = 0, w(sM , si) = dist(sM , si),w(gM , gi) = dist(gM , gi), w(si, ui) = w(ui, gi) = 0,

w(si, gj) = dist(si, gj).

With the construction, a TSP tour through GNO must usesMu0gM and all siuigi for all 1 ≤ i ≤ |RS |. To form acomplete tour, exactly (|RS |−1) edges of the form gisj , wherei 6= j must be used. At Line 2, the TSP is solved (usingConcorde TSP solver [63]). This yields a minimum weightsolution Sraw, which is a cycle containing all v ∈ VNO. Themanipulation actions can then be retrieved (Line 3).

An alternative solution to TORO-NO could employ theasymptotically optimal, SPLICE algorithm, introduced in [6].

A. Unlabelled TORO-NO

The scenario where objects are unlabeled is a special caseof TORO-NO which has significance in real-world applications(e.g., the pancake stacking application). This case is denotedas TORO-UNO (unlabeled, no overlap). Adapting the NP-hardness proof for the TORO-NO problem shows that cost-optimal TORO-UNO is also NP-hard. Similar to the TORO-NOcase, the optimal solution only hinges on the distance traveledby the manipulator because no buffer is required and exactlyn grasps and releases are needed.Theorem IV.2. Cost-optimal TORO-UNO is NP-hard.

Proof: See Appendix A.

When solving a TORO-UNO instance, Alg. 1 may beused with a few small changes. First, a different under-lying graph must be constructed. Denote the new graphas GUNO(VUNO, EUNO), where VUNO = RS ∪ RG ∪{sM , u0, gM}. For all 1 ≤ i, j ≤ n:

w(sM , u0) = w(gM , u0) = 0, w(sM , si) = dist(sM , si),w(gM , gi) = dist(gM , gi), w(si, gj) = dist(si, gj).

All other edges are given infinite weight. An example of theupdated structure of GUNO for two objects is illustrated inFig. 5.

sM

s1

s2

g1

g2 gM

u0

Fig. 5. An example of GUNO for 2 objects.

V. TORO WITH OVERLAP (TORO)

Unlike TORO-NO, TORO has a more sophisticated structureand may require buffers to solve. In this section, a dependencygraph [2] is used to model the structure of TORO, which leadsto a classical NP-hard problem known as the feedback vertexset problem [3]. The connection then leads to a completealgorithm for optimally solving TORO.

A. The Dependency Graph and NP-Hardness of TORO

Consider a dependency digraph Gdep(Vdep, Adep), whereVdep = O, and (oi, oj) ∈ Adep iff gi and sj overlap.Therefore, oj must be moved away from sj before moving oito gi. An example involving two objects is provided in Fig. 6.The definition of dependency graph implies the following twoobservations.Observation V.1. If the out-degree of oi ∈ Vdep is 0, then oican move to gi without collision.Observation V.2. If Gdep is not acyclic, solving TORO re-quires at least n+ 1 grasps.

The dependency graph has obvious similarities to the wellknown feedback vertex set (FVS) problem [3]. A directed FVSproblem is defined as follows. Given a strongly connected

6

s1g2

s2g1

o1 o2

(a) (b)

Fig. 6. Illustration of the dependency graph. (a) Two objects are to be movedfrom si to gi, i = 1, 2. Due to the overlap between s1 and g2 as well as theoverlap between s2 and g1, one of the objects must be temporarily movedaside. (b) The dependency graph capturing the scenario in (a).

directed graph G = (V,A), an FVS is a set of verticeswhose removal leaves G acyclic. Minimizing the cardinalityof this set is NP-hard, even when the maximum in degreeor out degree is no more than two [43]. As it turns out, theset of removed vertices in an FVS problem mirrors the setof objects that must be moved to temporary locations (i.e.,buffers) for resolving the dependencies between the objects,which corresponds to the additional grasps (and releases)that must be performed in addition to the n required graspsfor rearranging n objects. The observation establishes thatcost-optimal TORO is also computationally intractable. Thefollowing lemma shows this point.Lemma V.1. Let the dependency graph of a TORO problem bea single strongly connected graph. Then the minimum numberof additional grasps required for solving the TORO problemequals the cardinality of the minimum FVS of the dependencygraph.

Proof: Given the dependency graph, let the additionalgrasps and releases be nx and the minimum FVS have acardinality of nfvs, it remains to show that nx = nfvs. First,if fewer than nfvs objects are removed, which correspond tovertices of the dependency graph, then there remains a directedcycle. By Observation V.2, this part of the problem cannot besolved. This establishes that nx ≥ nfvs. On the other hand,once all objects corresponding to vertices in a minimum FVSare moved to buffer locations, the dependency graph becomesacyclic. This allows the remaining objects to be rearranged.This operation can be carried out iteratively with objects whosecorresponding vertices have no incoming edges. On a directedacyclic graph (DAG), there is always such a vertex. Moreover,as such a vertex is removed from a DAG, the remaining graphmust still be a DAG and therefore must have either no vertex(a trivial DAG) or a vertex without incoming edges.

For dependency graphs with multiple strongly connectedcomponents, the required number of additional grasps andreleases is simply the sum of the required number of suchactions for the individual strongly connected components.

For a fixed TORO problem, let nfvs be the cardinality of thelargest (minimal) FVS computed over all strongly connectedcomponents of its dependency graph. Then it is easy to seethat the maximum number of required buffers is no more thannfvs. The NP-hardness of cost-optimal TORO is establishedusing the reduction from FVS problems to TORO. This is moreinvolved than reducing TORO to FVS because the constructedTORO must correspond to an actual TORO problem in whichthe number of overlaps should not grow unbounded.

Theorem V.1. Cost-optimal TORO is NP-hard.Proof: The FVS problem on directed graphs is reduced

to cost-optimal TORO. An FVS problem is fully definedby specifying an arbitrary strongly connected directed graphG = (V,A) where each vertex has no more than two incomingand two outgoing edges. A typical vertex neighborhood canbe represented as illustrated in Fig. 7(a). Such a neighborhoodis converted to a dependency graph neighborhood of objectrearrangement as follows. Each of the original vertex vi ∈ Vbecomes an object oi which has some (si, gi) pair as itsstart and goal configurations. For each directed arc vivj , splitit into two arcs and add an additional object oij . That is,create new arcs oioij and oijoj for each original arc vij(see Fig. 7(b)). This yields a dependency graph that is againstrongly connected. Two claims will be proven:

1) The constructed dependency graph corresponds to anobject rearrangement problem, and

2) The minimum number of objects that must be movedaway temporarily to solve the problem is the same as thesize of the minimum FVS.

v1

v2v3

v4 o1

o2o3

o4

o31

o14

o12

o21

(a) (b)

Fig. 7. Converting a neighborhood of a graph for an FVS problem to partsof a dependency graph for a TORO problem.

To prove the first claim, assume without loss of generalitythat the objects have the same footprints on the tabletop. Fur-thermore, only the neighborhood of o1 needs to be inspectedbecause it is isolated by the newly added objects. Recall thatan incoming edge to o1 means that the start configuration o1blocks the goals of some other objects, in this case o21 and o31.This can be readily realized by putting the goal configurationsof o21 and o31 close to each other and have them overlap withthe start configuration of o1. Note that the goal configurationsof o21 and o31 have no other interactions. Therefore, suchan arrangement is always achievable for even simple (e.g.,circular or square) footprints. Similarly, for the outgoing edgesfrom o1, which mean other objects block o1’s goal, in thiscase o12 and o14, place the start configurations of o12 ando14 close to each other and make both overlap with the goalconfiguration of o1. Again, the start configurations of o12 ando14 have no other interactions.

The second claim directly follows Lemma V.1. Now, givenan optimal solution to the reduced TORO problem, it remainsto show that the solution can be converted to a solution tothe original FVS problem. The solution to the TORO problemprovides a set of objects that are moved to temporary locations.This yields a minimum FVS on the dependency graph but notthe original graph G. Note that if a newly created object (e.g.,oij) is moved to a temporary place, either object oi or oj canbe moved since this will achieve no less in disconnecting the

7

dependency graph. Doing this across the dependency graphyields a minimum FVS for G.Remark 5. It is possible to prove that TORO is NP-hard usinga similar proof to the TORO-NO case. To make the prooffor Theorem IV.1 work here, each pi can be split into anoverlapping pair of start and goal. Such a proof, however,would bury the true complexity of TORO, which is a muchmore difficult problem. Unlike the Euclidean-TSP problem,which admits (1+ε)-approximations and good heuristics, FVSproblems are APX-hard [3], [5].

B. Algorithmic Solutions for TORO

1) Feasible algorithm: Once the link between a TORObuffer requirement and FVS is established, an algorithm forsolving TORO becomes possible. To do this, an FVS set isfound. Then the optimal rearrangement distance is computedfor this FVS set. The procedure for doing this is outlined inTOROFVSSINGLE (Alg. 2). At Line 1, the dependency graphGdep is constructed. At Line 3-4, an FVS is obtained for eachstrongly connected component (SCC) in Gdep. Note that ifthese FVSs are optimal, then the step yields the minimumnumber of required grasps (and releases) as: min |A| =n+ |B|.

The residual work is to find the solution with n+|B| graspsand the shortest travel distance (Line 5). The manipulationactions are then retrieved and returned.

Algorithm 2: TOROFVSSINGLE

Input: Configurations sM , gM , Arrangements RS , RG

Output: A set of manipulation actions A1 Gdep ←CONSTRUCTDEPGRAPH(RS , RG)2 B ← ∅3 for each SCC in Gdep do4 B ← B ∪ SOLVEFVS(SCC)

5 Sraw ←MINDIST(sM , gM , RS , RG, Gdep, B)6 A ←RETRIEVEACTIONS(Sraw)7 return A

The paper explores two exact and three approximate meth-ods as implementations of SOLVEFVS() (Line 4 of Alg. 2).The two exact methods are both based on integer linearprogramming (ILP) models, similar to those introduced in[64]. They differ in how cycle constraints are encoded: oneuses a polynomial number of constraints and the other simplyenumerates all possible cycles. Denote these two exact meth-ods as ILP-Constraint and ILP-Enumerate, respectively.The details of these two exact methods are explained inAppendix A. With regards to approximate solutions, severalheuristic solutions are presented:

1) Maximum Simple Cycle Heuristic (MSCH). The FVSis obtained by iteratively removing the node that appearson the most number of simple cycles in Gdep until nomore cycles exist. The simple cycles are enumerated.

2) Maximum Cycle Heuristic (MCH). This heuristic issimilar to MSCH but counts cycles differently. For eachvertex v ∈ Vdep, it finds a cycle going through v andmarks the outgoing edge from v on this cycle. The

process is repeated for v until no more cycles can befound. The vertex with the largest cycle count is thenremoved first.

3) Maximum Degree Heuristics (MDH). This heuristicconstructs an FVS through vertex deletion based on thedegree of the vertex until no cycles exist.

Based on FVS, the solution minimizing travel distance canbe found by MINDIST() (line 5), which is an LP modelingmethod inspired by [23] and described in Appendix A.

2) Complete algorithm: Note that TOROFVSSINGLE() is acomplete algorithm for solving TORO but it is not a completealgorithm for solving TORO optimally. With some additionalengineering, a complete optimal TORO solver can also beconstructed: under the assumption that grasping dominates thetraveling costs, simply iterate through all optimal FVS sets andthen compute the subsequent minimum distance. After all suchsolutions are obtained, the optimal among these are chosen. Itturns out that doing this enumeration does not provide muchgain in solution quality as the optimal distances are verysimilar to each other.

VI. PERFORMANCE EVALUATION OF SIMULATIONS

All simulations are executed on an Intel(R) Core(TM) i7-6900K CPU with 32GB RAM at 2133MHz. Concorde [63]is used for solving the TSP and Gurobi 6.5.1 [65] for ILPmodels.

A. TORO-NO: Minimizing the Travel Distance

To evaluate the effectiveness of TORONOTSP, randomTORO-NO instances are generated in which the numberof objects varies. For each choice of number of objects,100 instances are tried and the average is taken. AlthoughTORONOTSP works on thousands of objects (it takes lessthan 30 seconds for TORONOTSP to solve instances with 2500objects), the evaluation is limited to 200 objects2. Concerningrunning time, TORONOTSP is compared with SPLICE [6]which does not compute an exact optimal solution. As shownin Fig. 8, it takes less than a second for TORONOTSP tocompute the distance optimal manipulation action set. Fig. 9

0 50 100 150 200Number of objects

0

2

4

6

8

10

Running tim

e (sec) ToroNoTSP

SPLICE

Fig. 8. Running time comparison of TORONOTSP and SPLICE.

illustrates the solution quality of TORONOTSP, SPLICE, andan algorithm that picks a random feasible solution. Noticethat the random feasible solution generally has poor quality.

2State-of-the-art Delta robots have comparable abilities. For example, theKawasaki YF03 Delta Robot is capable of performing 222 pick-and-placeactions per minute (1kg objects).

8

TABLE IEVALUATION OF THE TSP MODEL FOR THE UNLABELED CASE.

Number of objects 10 50 100 200Running time (sec) 0.04 0.58 2.43 7.30Optimality of random solution 1.94 3.72 4.92 6.01

SPLICE does well as the number of objects increases, butunder-performs compared to TORONOTSP. In conclusion,TORONOTSP provides the best performance on both runningtime and optimality for practical sized TORO-NO problems.

0 50 100 150 200Number of objects

1.0

1.2

1.4

1.6

1.8

Ratio

to optim

al so

lutio

n

ToroNoTSPSPLICERandom

Fig. 9. Optimality of TORONOTSP, SPLICE and a random selection method.

For the unlabeled case (TORO-UNO), the same experimentsare carried out. The results appear in Table I. Note thatSPLICE no longer applies. The last line of the table isthe optimality of random solutions, included for referencepurposes. For larger cases, the TSP based method is able tosolve for over 500 objects in 30 seconds.

B. TORO: Minimizing the Number of Grasps

To evaluate different FVS minimization methods, dependencygraphs are generated by capping the average degree andmaximum degree for a fixed object count. To evaluate therunning time, the average degree is set to 2 and the maximumdegree is set to 4, which creates significant dependencies. Therunning time comparison is given in Fig. 10 (averaged over100 runs per data point). Although exact ILP-based methodstook more time than heuristics, they can solve optimally forover 30 objects in just a few seconds, which makes them verypractical.

0 5 10 15 20 25 30 35Number of objects

0.0

0.5

1.0

1.5

2.0

Runn

ing

time

(sec

) ILP-ConstraintILP-EnumerateMSCHMCHMDH

Fig. 10. Running time of various methods for optimizing FVS.

When it comes to performance (Fig. 11), ILP-based methodshave no competition. Interestingly, the simple cycle basedmethod (MSCH) also works quite well and may be usefulin place of ILP-based methods for larger problems, given thatMSCH runs faster.

0 5 10 15 20 25 30 35Number of objects

1.0

1.2

1.4

1.6

1.8

Ratio

to optim

al so

lutio

n

ILP (optimal)MSCHMCHMDH

Fig. 11. Optimality ratio of various methods for optimizing FVS as comparedwith the optimal ILP-based methods.

The performance is also affected by the average degreefor each node, which is directly linked to the complexity ofGdep. Fixating on the ILP-Constraint algorithm, experimentswith an average degree of 0.5-2.5 are included (2.5 averagedegree yields rather constrained dependency graphs). As canbe observed from Fig. 12, for up to 35 objects, an optimalFVS can be readily computed in a few seconds.

0 5 10 15 20 25 30 35Number of objects

0

2

4

6

8

10

Runn

ing tim

e (sec

) Avg. deg.0.51.01.52.02.5

Fig. 12. The running time of ILP-Constraint under varying Gdep averagedegree. Maximum degree is capped at twice the average degree.

Finally, this section emphasizes an observation regarding thenumber of optimal FVS sets (Fig. 13). By disabling FVSs thatare already obtained in subsequent runs, all FVSs for a givenproblem can be exhaustively enumerated for varying numbersof objects and average degree of Gdep. The number of optimalFVSs turns out to be fairly limited.

5 10 15 20Number of objects

0

5

10

15

Numbe

r of F

VS so

lutio

ns

Avg. deg.0.51.01.52.02.5

Fig. 13. The number of optimal FVS solutions in expectation.

C. TORO: Overall Performance

The running time for the entire TOROFVSSINGLE() isprovided in Fig. 14. Observe that FVS computation takesalmost no time in comparison to the distance minimizationstep. As expected, higher average degrees in Gdep make thecomputation harder.

9

5 10 15 20Number of objects

0

25

50

75

100

Runn

ing tim

e (sec

) Avg. deg.0.51.01.52.02.5

Fig. 14. The total running time for TOROFVSSINGLE().

Running TOROFVSSINGLE() together with FVS enumera-tion, a global optimal solution is computed for TORO underthe assumption that the grasp/release costs dominate. Onlysolutions with an optimal FVS are considered. The computa-tion time is provided in Fig. 15. The result shows that it getscostly to compute the global optimal solution as the numberof objects go beyond 15 for dense setups. It is empiricallyobserved that for the same problem instance and differentoptimal FVSs, the minimum distance computed by MINDIST()in Alg. 2 has less than 5% variance. This suggests that runningTOROFVSSINGLE() just once should yield a solution that isvery close to being the global optimum.

5 10 15 20Number of objects

0

100

200

300

Total run

ning

time (sec

)

Avg. deg.0.51.01.52.02.5

Fig. 15. The running time to produce a global optimal solution for TORO.

VII. PHYSICAL EXPERIMENTS

Fig. 16. A snapshot of the physical system carrying out a solution generatedby the algorithms presented in this paper for a tabletop rearrangement scenario.

This section demonstrates an implementation of the al-gorithms introduced in this paper on a hardware platform

(Fig. 16). The physical experiments were conducted on threedifferent tabletop scenarios. For each scenario, multiple prob-lem instances were generated to compare the efficiency ofthe solutions produced by the proposed algorithms relative toalternatives, such as random and greedy solutions.

In order to apply object rearrangement algorithms in thephysical world, it is first necessary to identify the problemparameters corresponding to the formulation of the TOROproblem. Specifically, the algorithms are expecting as input astart and goal arrangement on a tabletop. The goal arrangementis predefined, while the objects are in an initial, arbitrary state.The physical platform first detects the starting arrangementof the objects before executing a solution for rearranging theobjects into the desired goal arrangement.

A. Hardware Setup

The experimental setup is comprised of five components:1. Tabletop The tabletop is a planar surface where the desktopmanipulator and objects rest. Clearly defined contours aredrawn denoting the projections of the manipulator’s reachablearea (i.e., workspace), including predefined buffers within thisreachable area, as shown in Fig 17.

Fig. 17. A tabletop environment where the uArm Swift Pro’s workspaceis the area between [45◦, 135◦] at a distance ranging between 140mm and280mm from the base.

2. Manipulator: The UFACTORY uArm Swift Pro 3

(Fig. 18a) is an inexpensive but versatile desktop manipulatorcapable of performing repeatable actions with a precision of0.2mm. The uArm Swift Pro’s versatility is largely due tothe variety of end-effectors that can be equipped. The suctioncup end-effector is utilized to achieve overhand grasps in theforthcoming challenges. Although the uArm Swift Pro is notbuilt for industrial applications, it has enough precision toperform the experimental tasks described in this section.3. Camera: The Logitech R©webcam C920 4 is a consumergrade webcam capable of Full HD video recording (1080p -1920×1080 pixels) that is used for object pose detection in theexperiments. The camera is mounted above the tabletop, suchthat it’s field-of-view (Fig. 17) captures the entire workspace.

3http://www.ufactory.cc/. The authors would like to thank uFactory forsupplying the uArm Swift Pro robot that was used in the hardware-basedevaluation.

4https://www.logitech.com/en-us/product/hd-pro-webcam-c920

10

(a) (b)Fig. 18. (a) The UFACTORY uArm Swift Pro grasping one of the labeled(colored) cylindrical objects. (b) An arrangement of 12 objects of variouscolor and geometry.

The pre-calibration eliminates lens distortion and also rendersbrighter, saturated images, which makes pose detection easier.4. Marker Detection “Chilitags” is a cross-platform softwarelibrary for the detection and identification of two-dimensionalfiducial markers [66]. Physical markers placed in the view ofthe imaging system are used as a point of reference/measurefor surrounding objects. In the case of Chilitags, that object is aphysical marker added to the environment. As seen in Fig. 17,the experiments employ five tags, each placed at a known poseon the tabletop. This knowledge facilitates the computationof the 2D transformation between the manipulator’s frame ofreference and the camera’s frame of reference.5. Objects There are several objects that the manipulatorinteracts with during the experiments (Fig. 18b). These objectsare identifiable according to their shape and color{cube, cylinder, orthotope} × {red, blue, green, yellow}

resulting in twelve uniquely identifiable objects. The centerof each object on the tabletop and its orientation define itsconfiguration.

B. Object Pose Detection

The predetermined goal configuration of the objects isdefined relative to the manipulator’s frame of reference. Theinitial configuration is unknown and is determined online viathe overhead C920 camera. Once the objects are detected,their observed configuration undergoes a transformation to themanipulator’s frame of reference.

The pose estimation method appears in Alg. 3. In line 2, animage is taken by the camera. Line 3 utilizes Gaussian blur(a.k.a. Gaussian smoothing) to reduce image noise. For each ofthe predefined colors (i.e., red, blue, yellow, green), the area(s)of the image matching the current color are extracted andfurther smoothed by morphological transformations, includingerosion and dilation. The contours of these areas, whichdescribe the top sides of objects, are then calculated (Line 5).Each contour is examined to determine whether or not itcorresponds to one of the objects in the scene (line 7). Foreach of the contours corresponding to an object in the scene,several operations need to occur to determine pose of theobjects. The 2D point component of the pose as it appearsin the camera is then determined by computing the center ofthe contour’s minimum enclosing circle (line 9), while theorientation of the object in the camera’s frame is extracted viaprinciple component analysis over the minimum area rectangle

containing the contour (line 10). Line 11 updates the 2D pointcomponent of the pose in the camera’s frame to accuratelyreflect position of the object relative to the manipulator, takinginto account the current shape geometry. The 2D perspectivetransformation between camera frame and robot frame is pre-computed using the marker detection software. The poses ofthe tags in the robot’s frame are fixed, but the pose of the tagsin the camera’s frame are automatically detected at runtime.

Algorithm 3: OBJECTPOSEESTIMATION

1 objects← {}2 img ← CAMERACAPTURE()3 img ← GAUSSIANBLUR(img)4 for color ∈ {red, blue, yellow, green} do5 contours← FINDCONTOURS(img, color)6 for contour ∈ contours do7 shape← DETECTSHAPE(contour)8 if shape ∈ {cube, cylinder, orthotope} then9 position←

CENTER(MINENCLOSINGCIRCLE(contour))10 orientation← PCA(MINAREARECT(contour))11 pose← UPDATE(position, orientation, shape)12 if INWORKSPACE(pose) then13 objects← objects ∪ {(shape, color, pose)}

14 return objects

C. Experimental Validation

This section presents three tabletop object rearrangementscenarios that can be performed via overhand pick-and-placeactions. For each scenario, specific problem instances havebeen provided that are solvable. The specific algorithm isdetermined at run-time, and corresponds to whether there isoverlap between the start and goal object configurations. If asubset of the start and goal configurations overlap, the problemmay require the use of external buffer(s). The number ofextra pick-and-place actions and the corresponding number ofbuffers necessary to carry out the task, can be determined viathe dependency graph.

All of the generated solutions by the proposed methodsperform an optimal number of pick-and-place actions. Asolution is optimal with respect to the travel distance ofthe end-effector when the external buffers are not utilized.Problem instances that require the use of an external buffer(s)remain near optimal with respect to the travel distance of theend-effector.

For each problem instance, a feasible solution is alsogenerated by either a random algorithm (for TORO-NO) ora greedy algorithm (for TORO5). The execution time for thedifferent solutions are then measured and compared.

Scenario 1: Cylinders without Overlap: This task re-quires the manipulator arm to transport four uniquely iden-tifiable cylinders from a random start configuration to a fixed

5The solution produced by the TOROFVSSINGLE algorithm uses thesolution returned by the Gurobi ILP solver after 10 sec of compute time.Empirically, any further computation only minimizes the transition timebetween poses at the expense of increased computation. Note that this doesnot affect the number of grasps which remain optimal.

11

(a) Start arrangement (b) Goal arrangementFig. 19. Problem instance of Scenario 1.

InstanceExecution Time (sec)

TORONOTSP Random Solution DifferenceRaw Tran Raw Tran1 65.04 19.81 68.50 23.27 3.462 70.44 25.21 75.38 30.15 4.943 67.11 21.88 69.85 24.62 2.744 69.34 24.11 73.09 27.86 3.755 70.67 25.44 73.04 27.81 2.376 71.13 25.90 73.37 28.14 2.247 73.40 28.17 73.66 28.43 0.268 67.95 22.72 67.97 22.74 0.029 66.62 21.39 68.21 22.98 1.59

10 66.88 21.65 68.22 22.99 1.34Average 68.86 23.63 71.13 25.90 2.27

TABLE IIEXPERIMENTAL RESULTS SHOWING A COMPARISON OF THE THE

EXECUTION TIME BETWEEN TORONOTSP AND A RANDOM FEASIBLESOLUTION FOR TEN PROBLEM INSTANCES.

goal configuration. Figures 19a and 19b illustrate one suchproblem instance where the start and goal configurationsdo not overlap, indicating that this is a TORO-NO instance.Since TORO-NO does not necessitate the use of any externalbuffers, the manipulator perform only four pick-and-placeoperations, corresponding to the number of cylinders in thescene. Appendix C provides the distance optimal sequence forpick-and-place operations of this instance.

To illustrate the efficiency of solutions generated byTORONOTSP, 10 different problem instances are generated.They are solved by both TORONOTSP and random graspingsequences. Note that any permutation of objects is a feasiblesolution. The result is presented in Table II. The first columnin this table specifies different problem instances. The rawcolumns measure the total execution time, which containsgrasps, releases, and transportation. Since the time for graspsand releases is the same for all the feasible solutions for aTORO-NO instance, the amount of time (45.23 sec) to performfour in-place pick-and-place actions is then deducted fromtotal execution time. These results appear in the tran columns.Moreover, the difference between the execution time of thealgorithms appear in the difference column.

From the empirical data provided in Table II, randomsolution strategies for this problem setup incur on average a9.6% increase in transportation time compared to the solutiongenerated by TORONOTSP.Remark 6. The difference of execution time between arandom solution and the optimal solution is generally propor-tional, but not linearly dependent to the transition cost definedin this paper, which is based on the Euclidean distance between

poses in 2D. This is due to the manipulator model. In practice,the path that the end-effector travels does not necessarily needto be along piece-wise linear shortest paths (straight lines).Additionally, the second order term (acceleration) varies, con-tributing to a non-static velocity, which is not captured by thetabletop model described herein.

(a) Start arrangement (b) Goal arrangementFig. 20. Problem instance of Scenario 2.

Scenario 2: Cylinders and Cubes: In this scenario, thearm is tasked with rearranging eight objects (four cylin-ders and four cubes) that are initially scattered throughoutthe workspace, while the desired goal configuration is pre-determined. Figures 20a and 20b show one such instance.Due to overlap between the start and goal configurations,this is a TORO instance and thus uses the TOROFVSSINGLEalgorithm, making use of the external buffers available tothe manipulator. In the solution of TOROFVSSINGLE on thisparticular instance, the manipulator uses one of the availablebuffer locations to perform the rearrangement. The movementof an object to an external buffer results in a total of ninepick-and-place actions.

TOROFVSSINGLE is compared to a greedy method, whichsolves the problems sequentially by first removing the depen-dencies for one object and then it moves it to its goal. Asshown in Table III, the greedy algorithm returns 1 extra graspsand takes 24.45 secs of additional execution time, comparedto TOROFVSSINGLE.

(a) Start arrangement (b) Goal arrangementFig. 21. Problem instance for Scenario 3 containing twelve objects.

Scenario 3: Cylinders, Cubes, Orthotopes: This scenarioinvolves rearranging twelve objects (four of each type ofcylinders, cubes and orthotopes) within the manipulator’sworkspace. Objects of identical geometry are aligned in frontof the manipulator, ordered by increasing height (i.e., ortho-topes, cubes, cylinders). Thus, start and goal configurationsdiffer by permutations of color amongst objects of the sameshape. The instance shown in Figures 21a and 21b utilizesthree of the available buffer locations as it performs sixteenpick-and-place actions necessary to solve the task.

As the number of objects increases, the difference betweenthe solution quality of TOROFVSSINGLE and the greedy al-gorithm becomes larger. The solution of greedy algorithm has

12

Scenario Execution Time (sec) Num. of ActionsTOROFVSSINGLE Greedy Alg. TOROFVSSINGLE Greedy Alg.

2 165.57 190.02 9 103 299.95 405.16 16 20

TABLE IIIEXPERIMENTAL RESULTS SHOWING A COMPARISON OF THE THE EXECUTION TIME BETWEEN TOROFVSSINGLE AND A GREEDY SOLUTION FOR TWO

SCENARIOS.

4 extra grasps and 105.21 secs more execution time comparedto the sub-optimal solution generated by TOROFVSSINGLE.

VIII. CONCLUSION

This paper studies the combinatorial structure inherent intabletop object rearrangement problems. For TORO-NO andTORO-UNO, it is shown that Euclidean-TSP can be reducedto them, establishing their NP-hardness. More importantly,TORO-NO and TORO-UNO can be reduced to TSP with littleoverhead, thus establishing that they have similar computa-tional complexity and lead to an efficient solution scheme.Similarly, an equivalence was established between dependencebreaking of TORO and FVS, which is APX-hard. The equiva-lence enables subsequent ILP-based methods for effectivelyand optimally solving TORO instances containing tens ofobjects with overlapping starts and goals.

The methods and algorithms in this paper serve as aninitial foundation for solving complex rearrangement taskson tabletops. Many interesting problems remain open in thisarea; two are highlighted here. The current paper assumesthe availability of external buffers, which are separated fromthe workspace occupied by the objects, demanding additionalmovement from the end-effector. In practice, it can be benefi-cial to dynamically locate buffers that are close by, which maybe tackled through effective sampling methods. Furthermore,the scenarios addressed only static settings whereas manyindustrial settings require solving a more dynamics problemin which the objects to be rearranged do not remain still withrespect to the robot base.

APPENDIX

Proof of Theorem IV.2: Again, reduce from Euclidean-TSP. The same TSP instance from the proof of Theorem IV.1is used. The conversion to TORO-NO and the process to obtaina TORO-UNO instance are also similar, with the exceptionbeing that edges sigi are not required to be used in a solution;this makes the labeled case become unlabeled.

The argument is that the cost-optimal solution of theTORO-UNO instance also yields an optimal solution to theoriginal Euclidean-TSP tour. This is accomplished by showingthat an optimal solution to the TORO-NO instance has essen-tially the same cost as the TORO-UNO instance. To see thatthis is the case, assume that an optimal solution (tour) path tothe reduced TORO-UNO problem is given. Let the path havea total length (cost) of DTORO−UNO

opt . Let sigi be the first suchedge that is not in the TORO-UNO solution. Because the pathis a tour, following si along the path will eventually reach gi.The resulting path will have the form siv1 . . . v2giv3, i.e., theblack path in Fig. 22.

si

v1 v2

gi v3. . .

Fig. 22. Augmenting a path in an TORO-UNO solution.

Upon the observation of such a partial solution, proceed tomake the augmentation and replace the path with the new one(red path in Fig. 22). Because sigi � 1/(4n), the potentialincrease in path length is bounded by (note that v2v3 is shorterthan the additive length of v2gi and giv3)

‖sigi‖2 + ‖giv1‖2 − ‖siv1‖2 ≤ 2ε� 1/(2n).

After at most n such augmentations, an optimal TORO-UNOsolution is converted to an TORO-NO solution. The TORO-NOsolution has a cost increase of at most n ∗ 1/(2n) = 1/2.The TORO-NO solution can then be converted to a solutionof the Euclidean-TSP problem, which will not increase thecost. Thus, a TORO-UNO solution can be converted to acorresponding Euclidean-TSP solution with a cost addition ofless than 1/2. Let the Euclidean-TSP solution obtained in thismanner have a total cost of D′, then

D′ < DTORO−UNOopt +

1

2. (4)

Now again let the optimal Euclidean-TSP solution have acost of Dopt. The solution can be converted to an TORO-NOsolution with a total cost of less than Dopt + 1/4. TheTORO-NO solution is also a solution to the TORO-UNOproblem. That is, for this new TORO-UNO solution, the costis

DTORO−UNO < Dopt +1

4. (5)

Now, if D′ > Dopt, then D′ ≥ Dopt+1. Putting this togetherwith (4) and (5),

DTORO−UNO < Dopt +1

4≤ D′ − 3

4≤ DTORO−UNO

opt − 1

4,

which is a contradiction. Therefore, D′ > Dopt cannot betrue. Therefore, a cost-optimal TORO-UNO solution yields anoptimal solution to the original Euclidean-TSP problem. Thisshows that TORO-UNO is at least as hard as Euclidean-TSP.

To compute the exact solution, the problem is modeled as anILP problem, and then solved using LP solvers, e.g., GurobiTSP Solver [65]. In this paper, two different ILP models are

13

used, which are similar to the models introduced in sections3.1 and 3.2 of [64]:

1) ILP-Constraint. By splitting all vertices oi ∈ Gdep

to oini and oouti , a new graph Garc(Varc, Earc) is con-structed, where Varc = {oin1 , oout1 , . . . , oinn , o

outn }, and

(oouti , oinj ) ∈ Earc iff (oi, oj) ∈ Adep. By adding extraedges (oini , o

outi ) for all 1 ≤ i ≤ n to Earc, problem

is transformed to a minimum feedback arc set problem,where the objective is to find a minimum set of arcs tomake Garc acyclic. Moreover, every edge in this set endsat oini or starts from oouti can be replaced by (oini , o

outi ),

which stands for a vertex oi in Gdep, without changingthe feasibility of the solution.The next step is to find a minimum cost ordering π∗ of thenodes in Garc. Let ci,j = 1 if edge (i, j) ∈ Earc, whileci,j = 0 if edge (i, j) /∈ Earc. Furthermore, let binaryvariables yi,j associate the ordering of i, j ∈ π, whereyi,j = 0 if i precedes j, or 1 if j precedes i. Suppose|Varc| = m, the LP formulation is expressed as:

miny

m∑j=1

(

j−1∑k=1

ck,jyk,j +

n∑l=j+1

cl,j(1− yj,l))

s.t. yi,j+yj,k−yi,k≤ 1, 1 ≤ i < j < k ≤ m−yi,j−yj,k+yi,k≤ 0, 1 ≤ i < j < k ≤ m

The solution arc set contains all the backward edges inπ∗.

2) ILP-Enumerate. First find the set C of all the simple cy-cles in Gdep. A set of binary variables V = {v1, . . . , vn}is defined, each assigned to an object oi ∈ O, the LPformulation is expressed as::

maxv

∑vi∈V

vi

s.t.∑

oi∈Cj

vi < |Cj |, ∀Cj ∈ C.

Then the vertices in the minimum FVS is the objectswhose corresponding variable vi is 0 in the solution ofthis LP model.

Given a TORO instance with n objects O = {o1, . . . , on},without loss of generality, B = {o1, . . . , op} ⊂ O denotesthe set of objects to be moved to buffers, which is calculatedby the methods introduced in Section V-B and Appendix A.The maximum number of buffers to be used is denoted asp = |B|. The ILP model introduced in this section finds thedistance-optimal solution amongst all candidates that movesobjects in B to intermediate locations before moving them togoal configurations while employing the minimum number ofgrasps (i.e. n+ p).

All variables in this ILP model are boolean variables, andhave two components: nodes and edges, where a node denotesthe occupancy of a position on the tabletop at a specific timestep, and an edge represents a movement of the manipulatorbetween two positions. The variables appear as a repeatedgraph pattern in a discrete time domain t ∈ {0, . . . , n + p},

where time step t correlates to the states of the system afterthe tth pick-and-place action.

Assume the following:

1 ≤ i ≤ n,1 ≤ j ≤ p,1 ≤ k ≤ p,1 ≤ ` ≤ n,p < m ≤ n.

A. Variables: Nodes

There are three kinds of nodes:• {st1, . . . , stn}, occupancy of start configurations. sti = 1

indicates oi is at its start configuration at time step t.• {gt1, . . . , gtn}, occupancy of goal configurations. gti = 1

indicates oi is at its goal configuration at time step t.• {bt11, bt12, . . . , btpp}, occupancy of buffers. btjk = 1 indi-

cates oj is in buffer bk at time step t.• sM , gM , indicate the occupancy of the manipulator’s rest

positions.The value of the nodes when t = 0 and t = n+ p indicate

the start and goal arrangement, respectively. Specifically,

s0i = 1, g0i = 0, b0jk = 0,

sn+pi = 0, gn+p

i = 1, bn+pjk = 0.

B. Variables: Edges

Edges model the movement of the manipulator and areincluded in the cost function. An edge connects two nodes,e.g. node 1 and 2, and is denoted as e(12). Edges in the ILPmodel are listed as follows.• e(stmg

tm), for 1 ≤ t ≤ n + p. A positive value indicates

a pick-and-place action that brings om from its startconfiguration to its goal configuration at time step t.

• e(stjbtjk), e(b

tjkg

tj), for 1 ≤ t ≤ n + p. A positive value

indicates a pick-and-place action that brings oj from itsstart configuration to buffer k, and from buffer k to itsgoal configuration.

• e(gtist+1` ), e(btjks

t+1i ), e(gtib

t+1jk ), for 1 ≤ t < n + p,

indicates a movement of the manipulator without anobject being grasped.

• e(sMs1i ), e(g

n+pi gM ), denote the target objects of the first

and the last pick-and-place actions.Note that because each edge is associated with a movement

of the manipulator, the total travel distance of the end-effectorcan be modeled as the summation of the value (i.e. 0 or 1)of all edges multiplied with the cost associated with the edge.The objective of this ILP model is

min∑e

cost(e),

where cost(e) denotes the distance between positions associ-ated with nodes connected by edge e.

An illustration of the ILP model is provided in Fig. 23.

14

C. Constraints

This section addresses the constraints that appear in the ILPmodel. These constraints update the value of variables, whichensures that the solution returned by the ILP model is able tobe reduced to a feasible solution for the original TORO.

b011

g01

g02

s11

g11

g12

s21

g21

s22

s31

b311

s32

sM gM

s01

s02

b111

s12

b211

g22

g31

g32

t = 0 t = 1 t = 2 t = 3

Fig. 23. An example of ILP model when n = 2, p = 1 as well as valueof variables after solving this model. The green square nodes and green solidedges indicate the value of variables is 1. The solution is interpreted as (i)bring o1 to a buffer, (ii) bring o2 to its goal, (iii) bring o1 to its goal.

Begin by considering the constraints of the first pick-and-place action. To update the occupancy of start configurations:

n∑i=1

e(sMs1i ) = sM = 1, s1i = s0i − e(sMs1i ).

To update the occupancy of buffers and goal configurations:p∑

k=1

e(s1jb1jk) = e(sMs

1j ), b

1jk = b0jk + e(s1jb

1jk),

e(s1mg1m) = e(sMs

1m), g1m = g0m + e(s1mg

1m).

After this step, the constraints for all other time steps canbe modeled. Assume the following: 1 ≤ t ≤ n+ p− 1.

The movement of the manipulator between time step t andt+ 1 correspond to:

e(stjbtjk) =

n∑i=1

e(btjkst+1i ) +

p∑a=1

p∑b=1

e(btjkbt+1ab ),

p∑j=1

e(btjkgtj) =

n∑i=1

e(gtjst+1i ) +

p∑k=1

p∑l=1

e(gtjbt+1kl ),

e(stmgtm) =

n∑i=1

e(gtmst+1i ) +

p∑k=1

p∑l=1

e(gtjbt+1kl ).

For time step t+1, constraints are imposed on the incomingedges from time step t, in order to avoid the scenario wherethe manipulator travels to a vacant location:

n∑i=1

e(gtist+1` ) +

p∑j=1

p∑k=1

e(btjkst+1` ) ≤ st`,

n∑i=1

e(gtibt+1jk ) +

p∑a=1

p∑b=1

e(btabbt+1jk ) ≤ btjk.

Update the edges to simulate a pick-and-place action in timestep t+ 1:

p∑k=1

e(st+1j bt+1

jk ) =

n∑i=1

e(gtist+1j ) +

p∑a=1

p∑b=1

e(btabst+1j ),

e(bt+1jk gt+1

j ) =

n∑i=1

e(gtibt+1jk ) +

p∑a=1

p∑b=1

e(btabbt+1jk ),

e(st+1m gt+1

m ) =

n∑i=1

e(gtist+1m ) +

p∑j=1

p∑k=1

e(btjkst+1m ).

Update nodes in time step t+ 1:

st+1i = sti −

n∑`=1

e(gt`st+1i )−

p∑j=1

p∑k=1

e(btjkst+1i ),

bt+1jk = btjk + e(st+1

j bt+1jk )

−n∑

i=1

e(gtibt+1jk )−

p∑a=1

p∑b=1

e(btabbt+1jk ),

gt+1j = gtj +

p∑k=1

e(bt+1jk gt+1

j ),

gt+1m = gtm + e(st+1

m gt+1m ).

Since each buffer is presented as multiple copies in eachtime step, one must make sure it is occupied by at most oneobject:

p∑j=1

btjk ≤ 1.

Update the dependencies in the dependency graph. Supposesi is in collision with g` then

sti + gt` ≤ 1.

After employed all n + p pick-and-place actions, the ma-nipulator goes to the rest position gM :

e(gn+pj gM ) =

p∑k=1

e(bn+pjk gn+p

j ),

e(gn+pm gM ) = e(sn+p

m gn+pm ),

gM =

p∑j=1

e(gn+pj gM ) +

n∑m=p+1

e(gn+pm gM ) = 1.

Fig. 24 demonstrates the optimal solution for the probleminstance shown in Fig. 19. The sequence of pick-and-placeactions over the colored cylinders are yellow, red, green, blue.

ACKNOWLEDGMENT

We thank uFactory for supplying the uArm Swift Pro robotthat was used in the hardware-based evaluation.

The authors would like to recognize the contributions of JiakunLyu {Zhejiang University, China} for his preliminary work onthe object pose detection used in the hardware experiments.

This work is supported by NSF awards IIS-1617744, IIS-1451737 and CCF-1330789, as well as internal support by

15

(a) (b)

(c) (d)Fig. 24. Step by step solution for the TORO-NO instance in Fig. 19.

Rutgers University. Any opinions or findings expressed in thispaper do not necessarily reflect the views of the sponsors.

A video containing complete executions for eachof the three scenarios described in the ExperimentalValidation (Section VII-C) can be found athttps://youtu.be/Ub7QSDQz0Qk.

REFERENCES

[1] C. H. Papadimitriou, “The Euclidean travelling salesman problem is NP-complete,” Theoretical Computer Science, vol. 4, no. 3, pp. 237–244,1977.

[2] J. van den Berg, J. Snoeyink, M. Lin, and D. Manocha, “Centralizedpath planning for multiple robots: Optimal decoupling into sequentialplans,” in Proc. Robotics: Science and Systems, 2009.

[3] R. M. Karp, “Reducibility among combinatorial problems,” in Complex-ity of computer computations. Springer, 1972, pp. 85–103.

[4] M. F. E. Rohmer, S. P. N. Singh, “V-REP: a Versatile and Scalable RobotSimulation Framework,” in Proc. IEEE/RSJ International Conference onIntelligent Robots and Systems, 2013.

[5] I. Dinur and S. Safra, “On the hardness of approximating minimumvertex cover,” Annals of Mathematics, pp. 439–485, 2005.

[6] K. Treleaven, M. Pavone, and E. Frazzoli, “Asymptotically optimal al-gorithms for one-to-one pickup and delivery problems with applicationsto transportation systems,” IEEE Transactions on Automatic Control,vol. 58, no. 9, pp. 2261–2276, 2013.

[7] S. D. Han, N. M. Stiffler, A. Krontiris, K. E. Bekris, and J. Yu, “High-Quality Tabletop Rearrangement with Overhand Grasps: Hardness Re-sults and Fast Methods,” in Proc. Robotics: Science and Systems, Boston,Massachusetts, U.S.A., Jul. 2017.

[8] B. Aronov, M. de Berg, A. F. van den Stappen, P. Svestka, andJ. Vleugels, “Motion Planning for Multiple Robots,” Discrete andComputational Geometry, vol. 22, no. 4, pp. 505–525, 1999.

[9] K. Solovey, J. Yu, O. Zamir, and D. Halperin, “Motion Planningfor Unlabeled Discs with Optimality Guarantees,” in Proc. Robotics:Science and Systems, 2015.

[10] K. Solovey and D. Halperin, “On the hardness of unlabeled multi-robotmotion planning,” in Proc. Robotics: Science and Systems, 2015.

[11] J. van den Berg and M. Overmars, “Prioritized Motion Planning for Mul-tiple Robots,” in Proc. IEEE/RSJ International Conference on IntelligentRobots and Systems, 2005, pp. 2217–2222.

[12] S. Leroy, J.-P. Laumond, and T. Simeon, “Multiple Path Coordination forMobile Robots: A Geometric Algorithm,” in Proc. International JointConferences on Artificial Intelligence, 1999, pp. 1118–1123.

[13] R. H. Wilson and J.-C. Latombe, “Geometric Reasoning about Mechan-ical Assembly,” Artificial Intelligence, vol. 71, no. 2, pp. 371–396, 1994.

[14] D. Halperin, J.-C. Latombe, and R. H. Wilson, “A General Frameworkfor Assembly Planning: the Motion Space Approach,” Algorthmica,vol. 26, no. 3-4, pp. 577–601, 2000.

[15] S. Sundaram, I. Remmler, and N. M. Amato, “Disassembly SequencingUsing a Motion Planning Approach,” in Proc. IEEE InternationalConference on Robotics and Automation, Washington, D.C., May 2001,pp. 1475–1480.

[16] D. Kornhauser, G. Miller, and P. Spirakis, “Coordinating Pebble Motionon Graphs, the Diameter of Permutation Groups, and Applications,” inProc. IEEE Symposium on Foundations of Computer Science, 1984, pp.241–250.

[17] G. Calinescu, A. Dumitrescu, and J. Pach, “Reconfigurations in Graphsand Grids,” SIAM Journal on Discrete Mathematics, vol. 22, no. 1, pp.124–138, 2008.

[18] V. Auletta, A. Monti, D. Parente, and G. Persiano, “A Linear TimeAlgorithm for the Feasibility of Pebble Motion on Trees,” Algorthmica,vol. 23, pp. 223–245, 1999.

[19] G. Goraly and R. Hassin, “Multi-Color Pebble Motion on Graphs,”Algorthmica, vol. 58, no. 3, pp. 610–636, 2010.

[20] G. Wagner, M. Kang, and H. Choset, “Probabilistic Path Planningfor Multiple Robots with Subdimensional Expansion,” in Proc. IEEEInternational Conference on Robotics and Automation, 2012.

[21] G. Sharon, R. Stern, A. Felner, and N. R. Sturtevant, “Conflict-basedSearch for Optimal Multi-agent Pathfinding,” in Artificial Intelligence,no. 219, 2015, pp. 40–66.

[22] J. Yu and S. M. LaValle, “Multi-agent Path Planning and Network Flow,”in Proc. Workshop on the Algorithmic Foundations of Robotics, 2012.

[23] ——, “Optimal Multi-Robot Path Planning on Graphs: Complete Algo-rithms and Effective Heuristics,” IEEE Transactions on Robotics, vol. 32,no. 5, pp. 1163–1177, 2016.

[24] G. Wilfong, “Motion Planning in the Presence of Movable Obstacles,”in Annals of Mathematics and Artificial Intelligence, 1991, pp. 131–150.

[25] P. C. Chen and Y. K. Hwang, “Practical Path Planning Among MovableObstacles,” in Proc. IEEE International Conference on Robotics andAutomation, May 1991, pp. 444–449.

[26] E. Demaine, J. O’Rourke, and M. L. Demaine, “Pushpush and push-1are NP-hard in 2D,” in Proc. Candadian Conference on ComputationalGeometry, 2000, pp. 211–219.

[27] D. Nieuwenhuisen, A. F. van der Stappen, and M. H. Overmars, “AnEffective Framework for Path Planning amidst Movable Obstacles,” inProc. Workshop on the Algorithmic Foundations of Robotics, 2006.

[28] J. van den Berg, M. Stilman, J. J. Kuffner, M. Lin, and D. Manocha,“Path Planning Among Movable Obstacles: A Probabilistically Com-plete Approach,” in Proc. Workshop on the Algorithmic Foundations ofRobotics, 2008.

[29] M. Stilman, J. Schamburek, J. J. Kuffner, and T. Asfour, “ManipulationPlanning Among Movable Obstacles,” in Proc. IEEE InternationalConference on Robotics and Automation, 2007.

[30] O. Ben-Shahar and E. Rivlin, “Practical Pushing Planning for Rearrange-ment Tasks,” IEEE Transactions on Robotics and Automation, vol. 14,no. 4, Aug. 1998.

[31] J. Ota, “Rearrangement Planning of Multiple Movable Objects,” in Proc.IEEE International Conference on Robotics and Automation, 2004.

[32] G. Havur, G. Ozbilgin, E. Erdem, and V. Patoglu, “Geometric Re-arrangement of Multiple Moveable Objects on Cluttered Surfaces: AHybrid Reasoning Approach,” in Proc. IEEE International Conferenceon Robotics and Automation, 2014.

[33] S. Srivastava, E. Fang, L. Riano, R. Chitnis, S. Russell, and P. Abbeel,“Combined Task and Motion Planning through an Extensible Planner-Independent Interface Layer,” in Proc. IEEE International Conferenceon Robotics and Automation, 2014.

[34] C. R. Garrett, T. Lozano-Perez, and L. P. Kaelbling, “FFRob: An efficientheuristic for task and motion planning,” in Proc. Workshop on theAlgorithmic Foundations of Robotics, 2014.

[35] A. Krontiris, R. Shome, A. Dobson, A. Kimmel, and K. E. Bekris,“Rearranging Similar Objects with a Manipulator using Pebble Graphs,”in Proc. IEEE International Conference on Humanoid Robotics, Madrid,Spain, 2014.

[36] A. Krontiris and K. E. Bekris, “Dealing with Difficult Instances ofObject Rearrangement,” in Proc. Robotics: Science and Systems, Rome,Italy, Jul. 2015.

[37] ——, “Efficiently Solving General Rearrangement Tasks: A Fast Ex-tension Primitive for an Incremental Sampling-based Planner,” in Proc.IEEE International Conference on Robotics and Automation, Sweden,2016.

[38] K. Hauser, “The Minimum Constraint Removal Problem with ThreeRobotics Applications,” International Journal of Robotics Research,vol. 33, no. 1, pp. 5–17, 2014.

[39] G. Berbeglia, J.-F. Cordeau, I. Gribkovskaia, and G. Laporte, “Staticpickup and delivery problems: a classification scheme and survey,” Top,vol. 15, no. 1, pp. 1–31, 2007.

[40] G. Berbeglia, J.-F. Cordeau, and G. Laporte, “Dynamic pickup anddelivery problems,” European Jornal of Operational Research, vol. 202,no. 1, pp. 8–15, 2010.

16

[41] G. Laporte, “The vehicle routing problem: An overview of exact andapproximate algorithms,” European Jornal of Operational Research,vol. 59, no. 3, pp. 345–358, 1992.

[42] N. Christofides and S. Eilon, “An algorithm for the vehicle-dispatchingproblem,” Journal of the Operational Research Society, vol. 20, no. 3,pp. 309–318, 1969.

[43] M. R. Garey and D. S. Johnson, Computers and Intractability: A Guideto the Theory of NP-Completeness. W. H. Freeman, 1979.

[44] S. Anily and R. Hassin, “The swapping problem,” Networks, vol. 22,no. 4, pp. 419–433, 1992.

[45] P. Beullens, D. van Oudheusden, and L. N. van Wassenhove, “Collectionand vehicle routing issues in reverse logistics,” in Reverse Logistics.Springer, 2004, pp. 95–134.

[46] A. Hoff and A. Løkketangen, “Creating lasso-solutions for the travelingsalesman problem with pickup and delivery by tabu search,” CentralEuropean Journal of Operations Research, vol. 14, no. 2, pp. 125–140,2006.

[47] I. Gribkovskaia, Ø. Halskau, G. Laporte, and M. Vlcek, “General solu-tions to the single vehicle routing problem with pickups and deliveries,”European Journal of Operational Research, vol. 180, no. 2, pp. 568–584,2007.

[48] G. N. Frederickson, M. S. Hecht, and C. E. Kim, “Approximationalgorithms for some routing problems,” in 17th Annual Symposium onFoundations of Computer Science (sfcs 1976). IEEE, 1976, pp. 216–227.

[49] T. Simeon, J.-P. Laumond, J. Cortes, and A. Sahbani, “Manipula-tion Planning with Probabilistic Roadmaps,” International Journal ofRobotics Research, no. 23, 2004.

[50] D. Berenson, S. S. Srinivasa, and J. J. Kuffner, “Task Space Regions: AFramework for Pose-Constrained Manipulation Planning,” InternationalJournal of Robotics Research, vol. 30, no. 12, pp. 1435–1460, 2012.

[51] J. B. Cohen, S. Chitta, and M. Likhachev, “Single- and Dual-arm MotionPlanning with Heuristic Search,” in International Journal of RoboticsResearch, 2013.

[52] M. Zucker, N. Ratliff, A. Dragan, M. Pivtoraiko, M. Klingensmith,C. Dellin, J. A. Bagnell, and S. S. Srinivasa, “CHOMP: CovariantHamiltonian Optimization for Motion Planning,” International Journalof Robotics Research, 2013.

[53] M. Ciocarlie and P. Allen, “Hand Posture Subspaces for DexterousRobotic Grasping,” in International Journal of Robotics Research,vol. 28, no. 7, 2009.

[54] J. Bohg, A. Morales, T. Asfour, and D. Kragic, “Data-driven GraspSynthesis - A Survey,” in IEEE Transactions on Robotics, vol. 30, no. 2,Apr. 2014.

[55] A. Cosgun, T. Hermans, V. Emeli, and M. Stilman, “Push Planningfor Object Placement on Cluttered Table Surfaces,” in Proc. IEEE/RSJInternational Conference on Intelligent Robots and Systems, 2011.

[56] M. R. Dogar and S. S. Srinivasa, “A Framework for Push-Grasping inClutter,” in Proc. Robotics: Science and Systems, 2011.

[57] S. Cambon, R. Alami, and F. Gravot, “A Hybrid Approach to IntricateMotion, Manipulation, and Task Planning,” International Journal ofRobotics Research, no. 28, 2009.

[58] E. Plaku and G. Hager, “Sampling-based motion and symbolic actionplanning with geometric and differential constraints,” in Proc. IEEEInternational Conference on Robotics and Automation, 2010.

[59] M. Gharbi, R. Lallement, and R. Alami, “Combining symbolic andgeometric planning to synthesize human-aware plans: toward moreefficient combined search,” in Proc. IEEE/RSJ International Conferenceon Intelligent Robots and Systems, 2015, pp. 6360–6365.

[60] N. T. Dantam, Z. K. Kingston, S. Chaudhuri, and L. E. Kavraki,“Incremental Task and Motion Planning: A Constraint-Based Approach,”in Proc. Robotics: Science and Systems, 2016.

[61] W. Vega-Brown and N. Roy, “Asymptotically optimal planning underpiecewise-analytic constraints,” in Proc. Workshop on the AlgorithmicFoundations of Robotics, 2016.

[62] G. N. Frederickson and D. J. Guan, “Nonpreemptive ensemble motionplanning on a tree,” Journal of Algorithms, vol. 15, no. 1, pp. 29–60,1993.

[63] D. L. Applegate, R. E. Bixby, V. Chvatal, and W. J. Cook, The TravelingSalesman Problem: A Computational Study. Princeton, NJ, USA:Princeton University Press, 2007.

[64] A. Baharev, H. Schichl, and A. Neumaier, “an exact method for theminimum feedback arc set problem,” University of Vienna, vol. 10, pp.35–60, 2015.

[65] I. Gurobi Optimization, “Gurobi Optimizer Reference Manual,” 2016.[Online]. Available: http://www.gurobi.com

[66] Q. Bonnard, S. Lemaignan, G. Zufferey, A. Mazzei, S. Cuendet, N. Li,A. Ozgur, and P. Dillenbourg, “Chilitags 2: Robust Fiducial Markersfor Augmented Reality and Robotics,” 2013. [Online]. Available:http://chili.epfl.ch/software