Cooperative Task and Motion Planning for Multi-Arm Assembly Systems

by   Jingkai Chen, et al.

Multi-robot assembly systems are becoming increasingly appealing in manufacturing due to their ability to automatically, flexibly, and quickly construct desired structural designs. However, effectively planning for these systems in a manner that ensures each robot is simultaneously productive, and not idle, is challenging due to (1) the close proximity that the robots must operate in to manipulate the structure and (2) the inherent structural partial orderings on when each part can be installed. In this paper, we present a task and motion planning framework that jointly plans safe, low-makespan plans for a team of robots to assemble complex spatial structures. Our framework takes a hierarchical approach that, at the high level, uses Mixed-integer Linear Programs to compute an abstract plan comprised of an allocation of robots to tasks subject to precedence constraints and, at the low level, builds on a state-of-the-art algorithm for Multi-Agent Path Finding to plan collision-free robot motions that realize this abstract plan. Critical to our approach is the inclusion of certain collision constraints and movement durations during high-level planning, which better informs the search for abstract plans that are likely to be both feasible and low-makespan while keeping the search tractable. We demonstrate our planning system on several challenging assembly domains with several (sometimes heterogeneous) robots with grippers or suction plates for assembling structures with up to 23 objects involving Lego bricks, bars, plates, or irregularly shaped blocks.



page 1

page 2

page 3

page 4


Anytime Stochastic Task and Motion Policies

In order to solve complex, long-horizon tasks, intelligent robots need t...

Anytime Integrated Task and Motion Policies for Stochastic Environments

In order to solve complex, long-horizon tasks, intelligent robots need t...

MAPS-X: Explainable Multi-Robot Motion Planning via Segmentation

Traditional multi-robot motion planning (MMP) focuses on computing traje...

Active Learning of Abstract Plan Feasibility

Long horizon sequential manipulation tasks are effectively addressed hie...

Learning to Design and Construct Bridge without Blueprint

Autonomous assembly has been a desired functionality of many intelligent...

Overview: A Hierarchical Framework for Plan Generation and Execution in Multi-Robot Systems

The authors present an overview of a hierarchical framework for coordina...

Graph-based Reinforcement Learning meets Mixed Integer Programs: An application to 3D robot assembly discovery

Robot assembly discovery is a challenging problem that lives at the inte...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Autonomous robots have been increasingly deployed to perform assembly tasks in factories. However, most robotic assembly is manually programmed, usually requiring months to accommodate new production needs. To be more flexible and adaptable to varying robot setups and products, we consider automated planning approaches to robotic assembly.

Planning for a team of robots to assemble a structure requires both (1) assigning every manipulation task to a robot and scheduling them subject to temporal constraints as well as (2) solving for motions that perform each manipulation while avoiding collisions between the moving robots and objects. These problems can be very challenging; even finding a feasible solution is non-trivial. First, the space of possible solutions is large: an industrial manipulator usually has at least six degree-of-freedoms (DOFs), and the joint state space of multiple robots and objects grows exponentially in the number of entities. Second, the assembly requirements in manufacturing typically involve a large number of different tasks (

e.g., assembling, holding, and welding) being performed over a long time horizon. Additionally, tasks are often tightly coupled with precedence constraints and concurrency requirements given the product properties. Third, the robots constantly operate in close proximity to each other in order to manipulate the structure and must avoid collisions with other moving robots and objects. Finally, an arbitrary feasible solution is typically not sufficient; minimizing solution makespan (i.e. total assembly time) is critical for efficiency and productivity in industrial applications.

Consider the example in Figure 1. Two robot arms R1 and R2 must detach and attach the bricks to move them from their start poses to their goal poses (the top left of Figure 1). The precedence-constrained tasks are given in the bottom left of Figure 1. In particular, after the blue brick (B) is assembled in its goal pose, one robot needs to hold B in order for the other robot to attach the red brick (R) (Steps 4 and 5 in the bottom right of Figure 1). A planner needs to find an assignment of these tasks (the top right of Figure 1) and plan paths that adhere to the assignments while avoiding three types of collisions (the bottom middle of Figure 1). The bottom right of Figure 1 sketches six key states on a solution.

Fig. 1: Phases of the proposed multi-robot task and motion planning algorithm on a two-robot Lego assembly problem.

We propose a task and motion planning framework to plan safe, low-makespan plans for multi-task multi-robot assembly problems, which addresses the above challenges. The structure of the framework is shown in Figure 1. This system takes as input the robot and assembly structure setup along with descriptions of the possible robot operating modalities and a goal description that consists of a set of tasks and precedence constraints between tasks. Our hierarchical planning system has three phases: (1) generate a multi-modal roadmap for each robot that describes its feasible movements and interactions with objects, and annotate colliding situations among different roadmaps; (2) optimally assign assembly tasks to robots based on their roadmaps by using Mixed-Integer Linear Programming (MILP) to solve a relaxed problem that still incorporates critical collisions when robots are performing manipulations; (3) deploy a priority-based, decoupled multi-agent search on the collision-annotated roadmaps to efficiently generate collision-free motion plans that fulfill the precedence-constrained tasks and respect the task assignments, which is biased towards plans with short makespan. Although the framework is a strict hierarchy that does not backtrack if no collision-free motion plans are found by (3), unlike many TAMP approaches, the high-level planner (2) directly considers aspects such as robot reachability, critical collisions, and precedence constraints, omitting only motion decision variables and their associated collision constraints, which substantially increase the size of the mathematical program. In practice, this not only produces feasible motion planning problems at the low level (3) but also ones that empirically admit low-makespan solutions. Finally, we tested our planning system’s ability to automatically generate low-makespan assembly solutions for complex assembly structures with multiple robot arms on challenging simulated assembly domains involving Lego bricks, bars, plates, and irregular-shaped blocks with grippers or suction plates as end-effectors for up to 23 objects.

Ii Related Work

Work in Multi-Modal Motion Planning (MMMP) addresses multi-step manipulation by planning across robot configuration spaces defined by changing manipulation modes [10, 9]. Building on MMMP, work in Task and Motion Planning (TAMP) bridges symbolic reasoning about actions that achieve discrete goals and geometric reasoning in search of collision-free robotic motions [5]. Most existing MMMP and TAMP methods are only able to plan for sequential systems, such as a single robot [5] or a team of synchronized robots [22], and are unable to represent plans where manipulation can happen asynchronously, for example when one robot places an object while other robots move according to their current manipulation modes. Existing algorithms for multi-agent TAMP [24] are capable of modeling multi-arm assembly problems but have not demonstrated the ability to solve TAMP problems with the long horizons and close robot proximity that are required in multi-arm assembly. By specializing in assembly problems, our system circumvents these challenges by using an efficient, collision-aware MILP formulation for high-level task assignment and leveraging ideas from state-of-the-art multi-agent path finding for low-level multi-modal motion planning.

Several other planning systems for multi-arm assembly have been developed for assembling furniture [14, 3], construction architecture[8], and LEGO bricks [20]. In some systems [14, 3], robots are restricted to move sequentially when installing new components. By using a decomposition approach that plans limited-horizon sub-problems in sequence, [8] is capable of finding solutions for up to 12 robots. However, their setting considers mobile manipulators operating in relatively open workspaces, so they are able to plan motions between identified manipulation keyframes using just a single-agent space-time motion planner. In contrast, we focus on planning for fixed-based robots working in crowded workspaces, in which finding jointly collision-free paths is very challenging, prompting us to build on tools from multi-agent path finding. In a crowded assembly setting similar to ours, [20] plans for three fixed-base robot arms to assemble up to 32 bricks, but they are unable to plan collision-free asynchronous motion for the robots.

Multi-Agent Path Finding (MAPF) plans for a group of agents, such as vehicles or drones, to reach specified goals without colliding. MAPF algorithms operate on finite graphs where each agent occupies exactly one vertex and can only move to adjacent vertices at each discretized time step. Recent work has significantly improved the scalability of MAPF algorithms [4] and generalized grid-world planning to incorporate task assignment [2, 11, 25]. Conflict-based search originates from MAPF and has been successfully applied to perform multi-arm motion planning [23]. Inspired by priority-based approaches [19, 25], our system explores task priorities to generate collision-free paths. Compared to classical MAPF, our system is able to plan paths that achieve multiple tasks and operate on general roadmaps with continuous traversal times.

Iii Problem Formulation

A multi-arm assembly planning problem is defined as the problem of planning trajectories for a team of possibly heterogeneous robots that manipulate a set of objects with respect to given assembly requirements, start and finish at specified configurations, avoid collisions among robots, assembly objects, and other obstacles. We consider holonomic robots that move subject to maximum velocity constraints. Each robot trajectory is a time-stamped path of time-configuration pairs . Velocities on the trajectory for must remain within the velocity limits. We minimize the makespan as our optimization criteria.

In our example in Figure 1, objects B and R must be detached from the base and then attached at a new location. During the process, B needs to be held while R is being attached. To describe such assembly requirements, we use mode graphs to qualitatively describe the procedures of the robots manipulating the objects. We use precedence-constrained tasks that leverages the mode graph representation to describe the assembly requirements.

A mode graph for a robot is a directed graph . Nodes are a set of modes that specify qualitative relations between a robot and the objects, each of which represents a set of robot configurations and the manipulated object states. Directed edges between nodes are motion primitives, each of which represents a set of moving or manipulating trajectories. A trajectory along with the manipulated object states can be qualitatively described as a time-stamped, interleaving sequence of modes and primitives. Figure 2

shows the mode graph of a robot manipulating objects B and R in our example. The modes classify states as free hand (Free), carry an object (Carry), and hold an object at its goal to be stable (HoldG). The primitives classify trajectories as move with free hand (Transit), transfer an object (Transfer), detach an object from its base or attach it at its target (Detach, Attach), and grasp or release an object at its goal (GraspG, ReleaseG).

Fig. 2: Mode graph.
Fig. 3: Precedence-constrained tasks.

The assembly requirement is given as a set of precedence-constrained tasks consisting of tasks T and precedence constraints P. Each task is a sequence of motion primitives that should be assigned to and achieved by a robot. Each constraint , where and are in the form of or , represents the start () or end () times of . Precedence constraints only specify necessary partial orderings instead of a total ordering. Figure 3 shows an example of precedence-constrained tasks. The first two are assembly tasks with primitives [Detach(B), Transfer(B), Attach(B)] and [Detach(R), Transfer(R), Attach(R)]. The other one is a support task [GraspG(B), ReleaseG(B)]. There are three types of precedence constraints in the example: (1) the precedence constraints between subsequent modes or primitives in the same task (e.g., Detach(B) precedes Transfer(B)); (2) Detach(B) precedes Detach(R), Attach(B) precedes Attach(R), and Attach(B) precedes GraspG(B) given the assembly structure; (3) Attach(R) precedes Grasp(B) and succeeds ReleaseG(B) given the support requirement.

In this paper, we assume: (1) each object can only be manipulated by one robot at a time, and thus lifting or handover among robots are not supported; (2) objects are relatively light compared to the robot payload, and objects are solid; (3) the assembly process is monotonic and thus objects do not need to be placed at intermediate locations for regrasping.

Iv Roadmap Generation

Given a robot and its mode graph , we generate a multi-modal Probabilistic Roadmap that describes its feasible movements and interactions with the objects. Vertices are a set of configurations. Each vertex is labeled with a mode and, when relevant, a grasp pose that describes the relative transformation between the robot’s end-effector and the manipulated object. Let if . Directed edges are a set of configuration trajectories. Each edge from vertex to vertex is labelled with a traversal time of this configuration trajectory given the maximum joint velocities, a primitive and a grasp pose . Let if . These vertices and edges are generated to be free of robot self-collisions and collisions with static obstacles.  Figure 4 shows an example multi-modal roadmap.

We sample the multi-modal roadmap in a manner similar to the general MMMP sampling method in [9], which iteratively samples in the mode configuration spaces and their intersections. In our mode graph, primitives are either mode-changing (e.g., Detach, Attach, GraspG, ReleaseG) or mode-preserving (e.g.,, Transit, Transfer). We take a task-aware sampling method to take advantage of the structure of assembly problems. Edges and vertices are sampled as follows: (1) we first use manipulation skill samplers to generate a diverse set of trajectories as edges called mode-changing edges (i.e. the non-black edges in Figure 4) for mode-changing motion primitives, and their starts and ends are milestones of the corresponding modes (e.g., the solid-line circles); (2) then we sample mode-preserving edges and vertices for the mode-preserving primitives and the pointed modes, which also connect the previously sampled milestones.

These edges and vertices compose a single-mode roadmap (e.g., Transit-Free roadmap and Transfer-Carry roadmap) of a mode-preserving primitive and its pointed mode (e.g., the black lines and the dashed circles). We have two ways to sample single-mode roadmaps: (2a) we use roadmap spanners [13], which are connected to the milestones; (2b) we also use RRT-Connect [15] to find paths (i.e. an interleaving sequence of edges and vertices) between milestones as highways. Vertices in (2a) and (2b) are connected together via connection edges to enhance connectivity. Because roadmap vertices also differ in grasp poses, a single-mode roadmap can have disconnected components under different poses. For example, grasping B from the top or side results in two disjoint components in Carry(B), as in Fig.4.

In a multi-modal roadmap, mode-changing edges (step 1) and highways (step 2b) capture the fastest paths for a robot to complete tasks while the spanned vertices and edges (step 2a) serve as alternatives when the highways are blocked by other robots during a time window. To reduce the roadmap size, we sample and add these edges differently for task assignment and path finding. In task assignment, since we only consider inter-robot collisions of mode-changing edges, we compute a multi-modal roadmap only consisting of mode-changing edges and highways. Then, when the tasks are assigned, we compute a multi-modal roadmap consisting of all the spanned vertices, spanned edges, and highways that are related to the assigned tasks, which are connected via corresponding connection edges. As each object has a unique Carry-Transfer roadmap, the number of spanned vertices and edges increases linearly in the number of objects. Thus, to further reduce the generation time, we cache the arm configurations and collision information in the Transit-Free roadmap and reuse them for spanning other single-mode roadmaps for the same robot. As a result, a large number of spanned components share the same arm configurations.

Fig. 4: Multi-modal roadmap (most edges in single-mode roadmaps such as Transit-Free and Transfer-Carry are omitted).

Iv-a Collision Annotation

Although each robot and its manipulated object are guaranteed to avoid collisions with the fixed obstacles when traversing its roadmap, they also need to avoid colliding with the other robots and objects. We adopt the idea of annotated collisions in [12] to characterize pairwise collisions between robot and robot, robot and object, or object and object in our assembly problem. Each annotated collision is a pair of conditions, where each condition denotes an area swept by a robot or an object in the workspace, and the annotated collision implies that the two areas overlap. Specifically, we have two conditions types: (1) edge condition and vertex condition represent the swept area of robot and its manipulated object when traversing edge or waiting at vertex respectively; (2) object condition or represents the area occupied by object being at its start () or goal pose () respectively. With all the roadmaps, we collect the conditions for all the robots and objects. Then, we do pairwise collision checks between them to record the colliding pairs as annotated collisions. We say there is a collision when a pair of annotated conditions both hold true at the same time. As a large portion of vertices and edges in roadmaps share the same arm configurations, they sweep the same area, and the collisions between them and others are only checked once.

V Task Assignment

The task assignment module generates a plan in which each robot performs a sequence of tasks such that all assembly tasks are assigned with respect to the assembly requirements and roadmap connectivity. The optimizing criteria is minimizing the plan makespan. The task assignment problem at this stage is a relaxation of the full problem since we ignore potential collisions when robots traverse through non-milestones. However, the inclusion of some collision constraints as well as non-trivial lower bounds on path-traversal durations ensures that this relaxation is representative of the full problem. Fully collision-free paths will be generated by our multi-task multi-agent path finding algorithm by refining a set of partially ordered, unscheduled subplans extracted from our task assignment solution (Section VI). This task assignment problem can be treated as an extension of Vehicle Routing Problems with Time Windows with exclusion constraints [16] and formulated as a Mixed-Integer Linear Program (MILP) [2].

(a) Task roadmap
(b) Plan roadmap
(c) Subplan sequences: nodes denote modes and blocks denote primitives.
Fig. 8: Task roadmap, plan roadmap, and subplan sequences.

V-1 Task Roadmap

For every task and every robot , we extract a task roadmap , an acyclic directed graph that represents all the paths to complete task . We construct by (1) adding all the milestones and mode-changing edges introduced by primitives to ; and (2) in each mode, adding a mode-preserving edge from every milestone that ends a mode-changing edge to every milestone that starts a mode-changing edge with the minimum traversal time as the weight except for mode Free. We compute these weights by using the Floyd-Warshall algorithm. The added edge is an abstraction of the highways between them and thus has the same primitive. A task roadmap example is in Fig.8(a).

V-2 Plan Roadmap

For every robot , we construct a plan roadmap to represent all the paths of to complete tasks T. To construct , we first compose all the task roadmaps . We find the root vertices of all the task roadmaps along with the robot goal vertex as the plan roots, and the leaf vertices along with the robot start vertex as the plan leaves. Roots and leaves have zero in-degree and out-degree respectively. Then, we add an edge from every leaf to every root with its traversal time except for the vertices belonging to the same task roadmap. The nodes of this plan roadmap are not necessarily a subset of the multi-modal roadmap since some tasks are required to executed more than once. An example of plan roadmap is given in Fig.8(b).

V-3 MILP Encoding

For every robot and every edge

, we create a binary variable

to indicate that traverses edge and a non-negative real variable to denote the times of arriving . For every primitive , we use real variables and to denote the start and end times of . Let and also denote the same variables as and for if is labeled with . We define a real variable to denote the total makespan. For a vertex , we denote its incoming edges in as and the outgoing edges as . The implication logical operator in the MILP model can be compiled to linear constraints by using the big-M method [6].


Constraints (1-2) ensure every robot traverses through a valid path in its plan roadmap from start to goal , and (3) enforces the arrival times of the vertices on this path to respect traversal times. (4) constrains each task to be assigned to exactly one robot, and (5) links the start and end times of each task primitive with the arrival times of its assigned robot. (6) enforces these start and end times to satisfy the precedence constraints P, and makespan an upper bound on all the time variables. Thus, constraints (1-6) ensure the robot paths complete all the tasks on time while respecting the roadmap connectivity and traversal times.

Constraints (7-9) prevent robots from colliding with objects or each other when taking mode-changing edges, which are often manipulation actions and more likely lead to dead ends. Constraint (7) enforces the mode-changing edges that collide with each other not to happen concurrently, and (8) guarantees such edges that collide with objects at starts or goals are not taken before detaching or after attaching objects respectively. These constraints are paramount to producing task assignments that admit full collision-free motion plans and empirically mitigating the incompleteness of our hierarchical design. For example, in Figure 1, the robot on the right can only grasp object R from the top. If the MILP did not have constraint (9), the planner might assign this robot to assemble object R. However, since B, which is attached before R, blocks R from being top grasped, this task assignment admits no feasible motion plan. Indeed, in practice, without these constraints, the system fails to produce a solution for most assembly problems of interest.

V-4 Extracting Subplan Sequences

A MILP solution produces a path for each robot, see the gray shadow line in Fig.8(b). From a solution, we extract a subplan sequence for each robot as in Fig. 8(c). Each subplan is a mode or primitive associated with the start and end vertices as assigned in the MILP solution. A subplan is planned if a time-stamped path from its start vertex to its end vertex on roadmap is provided. The subplans can be classified as three types: (1) a mode subplan has the same start and end vertices and thus its path is a single vertex; (2) implicitly, the path of a mode-changing primitive subplan can only traverse its corresponding edge in the MILP solution; and (3) the other primitive subplans need to plan longer paths such as Transit and Transfer primitives as sketched in Fig. 8(c). We extract such subplan sequences for all robots to obtain and add precedence constraints between the subplan end times according to the original assembly requirement. Then, these assigned, unscheduled, partially ordered subplans become the goal description of our path finding problem in Section VI.

1 0.9 ;
2 ;
3 while   do
4       while  do
5             if ) is false then
6                  return ;
7            if  is false then
8                  go to Line 1;
9            foreach  do ;
10             ;
12       subplans involved in the 1st collision in ;
13       foreach  do
14             ;
15             if  is true then .insert();
18return false;
Algorithm 1 PBS-AT

Vi Path Finding with Assigned Tasks

We now introduce our Priority-Based Search algorithm for multi-robot path finding with Assigned Tasks (PBS-AT), which plans paths for every robot to fulfill its subplan sequence on its multi-modal roadmap such that the precedence constraints are satisfied and the paths are collision-free given annotated collision . The idea of PBS-AT is to divide a multi-robot problem into single-robot sub-problems and explore the priorities of planning sub-problems as proposed in PBS with Precedence Constraints (PBS-PC) [25]. PBS-PC is a two-level algorithm that plans collision-free paths for multiple robots from their starts to visit sequences of precedence-constrained goals in grid graphs (i.e., MAPF-PC). As its high level explores the priorities between goals in a Priority Tree (PT) such that the robots that move towards lower-priority goals should avoid colliding with those that move towards higher-priority goals, its low level uses A* to plan single-robot paths optimally in discretized timesteps by reserving the paths of higher-priority goals as moving obstacles. Similar to PBS-PC, PBS-AT is also a two-level algorithm that explores the priorities between goals (subplans) and calls its low level to plan paths for subplans. Additionally, its low-level plans paths for multiple successive same-robot subplans at once instead of one subplan at a time. For simplicity, in this section, we use , , instead of to denote a subplan, where subplans for different robots have different index values , and to denote its corresponding path.

The high level of PBS-AT (Algorithm 1) performs a depth-first search on the PT. It starts with the root PT node that contains an empty set of paths, an empty set of collisions, and the initial priority orderings , which are initialized with respect to the precedence constraints to enforce subplans that end later to have lower priorities (Line 1). The precedence constraints between subsequent same-robot subplans are trivially included. Then, a stack is initialized with the root node (Line 1). When expanding PT node (Line 1), it first plans paths for unplanned subplans one at a time with respect to the priority orderings (Algorithms 1 to 1), i.e. NextUnplannedSubplan always returns the unplanned subplan that does not have any unplanned higher-priority subplans (Algorithm 1), until (1) some collisions are found (Algorithm 1), (2) all paths are planned, in which case we return the paths (Algorithm 1), or (3) no paths exist, in which case we prune (Line 1). PlanPaths() returns a set of paths because it plans a path for and, if necessary, replans paths for the previous same-robot subplans of . Last, PBS-AT resolves a collision in in the same way as PBS-PC and replans the paths in each generated child node by calling UpdateNode (Algorithms 1 to 1).

0.9 ;
  // sort subplans to replan in in order of
1 while  do
2       if livelock occurs then return false;
3       if  is false then return false;
4       foreach  do ;
5       ;
6       ;
8return true;
Algorithm 2 UpdateNode (Node , subplan )

UpdateNode (Algorithm 2) iteratively updates the paths of all the affected lower-priority subplans until all planned paths in : (1) satisfy the precedence constraints, (2) do not collide with any objects, and (3) any two planned paths that have priorities in between are collision-free. It first constructs a priority queue to store all the subplans to replan, in which subplans are sorted according to (Algorithm 2). It then repeatedly calls PlanPaths to replan until no more subplans need to be replanned (Line 2), a live lock occurs (Line 2), or a failure is reported by PlanPaths (Line 2). A live lock is a condition where updating a set of subplans triggers replanning of each other in a loop and leads to infinite replanning. In each iteration, when PlanPaths replans paths successfully, PBS-AT updates accordingly (Algorithm 2), adds the lower-priority subplans that either violate the precedence constraints due to the updated times of the replanned subplans or collide with the updated paths to (Algorithm 2), and deletes the subplans that have been replanned in this iteration from (Algorithm 2).

PlanPaths( plans an optimal path for that (1) avoids collisions with the objects and the paths of higher-priority subplans; and (2) ends after the end time of any subplan that must end earlier than . In the case that the subsequent same-robot subplans of already have paths, PlanPaths replans their paths accordingly to avoid disjoining the paths of two subsequent subplans over time. Moreover, if there does not exist a path for or its subsequent subplans, PlanPaths backtracks and replans the previous subplan of . This backtracking procedure is repeated until PlanPaths successfully finds paths for all the subplans that it has to plan, in which case it returns true, or no more previous subplan exists, in which case it returns false. In each backtracking iteration with newly added subplan , PlanPaths calls recursive SIPP (rSIPP) with , and rSIPP will plan paths for and all its subsequent same-robot subplans that already have paths.

1 0.9 should precede ;
2 generate root node at at time and insert it to ;
3 while  do
4       if  then
5             extract the path from ;
6             if  then
7                  Add a wait till time action to ;
9             the subsequent subplan of ;
10             if  is none or  then return ;
11             ;
12             foreach  do
13                   if   then
14                         Add a wait till time action to ;
15                         return ;
19      expand node w.r.t.  and insert its child nodes to ;
21return ;
Algorithm 3 rSIPP(Node , subplan , time , RT )
(a) Lego Bridge
(b) Puzzle Vault
(c) Truss Boat
(d) Card House
Fig. 13: Problem instances. (a) Lego Bridge: two robots with grippers assemble 17 Lego bricks; (b) Puzzle Vault: three robots with suction plates assemble 14 irregular-shaped blocks; (c) Truss Boat: three robots with grippers assemble 16 bars; (d) Card House: a robots with a gripper and a robot with a suction plate assemble 23 plates.
Domain Roadmap Generation & Annotation MILP-based Task Assignment PBS-AT
# # (min) (hr) #B #X #C (s) # #N #rSIPP (s)
(a) 8450 13454 87 741 2985 83 95795 40/44/65 166 6 214 66 0.04
(b) 4973 8642 103 1289 1148 57 2853 3/10/11 129 29 278 32 0.22
(c) 4049 8688 86 1035 18857 88 51224 90/175/496 153 44 221 122 0.18
(d) 8188 12818 112 861 4253 141 13157 71/105/821 190 11 221 17 0.03
TABLE I: Simulation Results. Average vertex (#) and edge (#) number of highways and connections; : average roadmap generation time; : roadmap annotation time; number of binary variables (#B), continuous variables (#X), and constraints (#C) in the MILP; runtimes to find a feasible MILP solution (), find an optimal solution (), and exhaust the solution space (); #g: number of subplans; #N: number of expanded nodes; #rSIPP: calls to rSIPP; : PBS-AT runtime; : ratio of the used annotated collisions.

Safe Interval Path Planning (SIPP)[21] is a variant of A* that finds an path with minimal total traversal time that avoids moving obstacles. We adapt it to rSIPP (Algorithm 3) so that it finds a set of paths for successive subplans that avoid the moving obstacles, i.e. the objects and the higher-priority subplans, and satisfies the precedence constraints. It takes input a Reservation Table (RT) as in SIPP that reserves the time intervals at each vertex that are occupied by the moving obstacles. The unreserved time intervals are called safe intervals. rSIPP searches in the resulting vertex-safe-interval graph to find an optimal path that (1) visits each vertex within a safe interval, (2) does not collide with any moving obstacles when it traverses an edge, and (3) ends no earlier than , where is the minimum allowable time to finish this subplan with respect to all the subplans that should precede it (Algorithm 3). rSIPP’s has the same search procedure as SIPP except for the goal test. When rSIPP finds a goal node (Algorithm 3), it extracts the path from (Algorithms 3 to 3), checks whether is the last subplan to replan (Algorithms 3 to 3), and if so terminates. Otherwise, it plans for the subsequent subplan to ensure a path starting from the end of exists (Algorithms 3 to 3). Specifically, it checks each reachable safe interval at the end vertex with respect to the RT for , calls rSIPP for each of them, and, if succeed, returns the found paths together with .

Like the PBS-PC algorithm, PBS-AT is incomplete but quite effective in practice. Additionally, PBS-AT can explore all possible priority orderings and is biased towards solutions with a shorter makespan. Meanwhile, instead of only planning each subplan optimally, rSIPP also tries to optimize the paths back and forth in a non-myopic way.

Vii Simulation Results

In our implementation of roadmap generation and collision annotation, we leverage PyBullet Planning to generate single-mode roadmaps, check collisions, and simulate skill trajectories111 We solved the MILP formulation using Gurobi [7]. The PBS-AT algorithm is implemented in C++. We tested our implementation on a 3.40GHZ 8-Core Intel Core i7-6700 CPU with 36GB RAM and leveraged 100 CPUs each with 8G memory on Amazon Web Service (AWS) to compute annotated collisions in parallel. In all the domains, we use Kuka KR-6-R900 arms with grippers or suction plates.

The skill samplers search 24 different grasps for Lego bricks and 8 for other objects to generate collision-free manipulation trajectories. We generate 500 vertices for the Free-and-Transit roadmap by using the k-nearest neighbors PRM with

. This generation takes roughly 40 minutes in all the domains. The maximum number of RRT-Connect samples is set to 3000. The highway vertices are connected to its 20 nearest spanned vertices via connection edges. The maximum edge duration is 0.1s, and longer edges are interpolated to sequences of edges. The joint resolution for collision checking is 0.01


We test our examples on domains with different features as shown in Fig. 13: (a) Lego Bridge: two robots with grippers need to detach 17 Lego bricks from the right and assemble them as a bridge, during which necessary holding actions are provided to keep Lego bricks stable; (2) Puzzle Vault: three robots with suction plates assemble vault with 14 irregular-shaped blocks; (3) Truss Boat: three robots with grippers assemble a bridge with 16 bars in a narrow space; (4) Card House: a heterogeneous team of two robots, one with a gripper and the other with a suction plate, cooperate to construct an assembly with 23 plates, In (4), the planner must account for the differing robot abilities, such as only the robot with the suction plate can place plates at the bottom. In the last three domains, we assume (1) objects spawn at the top of the construction, which abstracts the operations of some cranes or conveyors delivering the objects, for robots to pick; (2) objects are glued to the structure when they are assembled. All the domains originate the real-world designs and the precedence-constrained tasks are extracted given the design structure. Videos of a solution to each domain are provided in

The runtime results and statistics of each phase (Sections IV, V, VI) are reported in TABLE I. For roadmap generation and annotation, we only report the generation time of the roadmaps that are used by PBS-AT and skip the statistics for the roadmap used by task assignment. The latter roadmap of each robot is of a much smaller size and takes less than 20 minutes to generate and annotate. We report the average number of sampled vertices and edges for highways and connection edges over all the robots (#V, #E). We also report the runtime to generate this multi-modal roadmap () and the runtime to annotate all the necessary collision pairs (). This also includes the time to generate and annotate the shared configurations in Transit-Free roadmaps. As we can see in the table, the time to generate roadmaps is around 100 minutes for each robot and the annotation time can be around 1000 hours. Although we leverage CPU clusters for parallel computation which reduces the total runtime to a couple of hours, this time can likely be significantly improved in future work by using (1) lazily roadmap generation and annotation methods [1, 18] or (2) voxel-based collision checking on a GPU[17].

In the MILP-based task assignment, we report the numbers of binary variables (#B), continuous variables (#X), and constraints (#C) along with the runtimes to find the first solution (), find the the optimal solution (), and exhaust the solution space (). As in TABLE I, all the MILP encodings feature a relatively small number of continuous variables and can have up to 18000 binary variables. #B is dominated by the number of mode-changing edges and the potential collision between these edges. Thus, we can see Truss Boat has the largest #B since its mode-changing edges are very likely to collide with each other or the objects. Although these MILPs are large, Gurobi can find a feasible solution in two minutes and then an optimal one in a couple of minutes for all the domains, which is due to the relatively small number of continuous variables in all the domains.

In the PBS-AT column, we show the number of subplans (#g), expanded nodes (#N), and calls to rSIPP (#rSIPP), the runtime to find the solutions (), and the ratio of the used annotated collisions over all the annotated collisions (). As we can see, all the domains can be solved in two minutes with up to 44 node expansions. In crowded domains such as Puzzle Vault and Truss Boat, in which robots are likely to collide, PBS-AT needs to expand more nodes and add more priorities, and can be up to 22%. However, in domains where robots are facing each other and do not collide often, can be low as 3%. As we can see, our task assignment and path finding algorithms can quickly find low-makespan solutions in a couple of minutes; the runtime is dominated by roadmap generation and collision annotation.

Viii Conclusion and Future Work

We present a task and motion planning framework that jointly plans safe, low-makespan plans for multiple, possibly heterogeneous robot arms to assemble complex structures. We demonstrate its effectiveness in several simulated assembly domains. Future work includes (1) incorporating advanced cooperative behaviors such as handovers; and (2) improving the roadmap generation and collision annotation by leveraging lazy and GPU-based collision checking.


  • [1] R. Bohlin and L. E. Kavraki (2000) Path planning using lazy prm. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), Vol. 1, pp. 521–528. Cited by: §VII.
  • [2] K. Brown, O. Peltzer, M. A. Sehr, M. Schwager, and M. J. Kochenderfer (2020) Optimal sequential task assignment and path finding for multi-agent robotic assembly planning. In 2020 IEEE International Conference on Robotics and Automation (ICRA), pp. 441–447. Cited by: §II, §V.
  • [3] M. Dogar, A. Spielberg, S. Baker, and D. Rus (2019) Multi-robot grasp planning for sequential assembly operations. Autonomous Robots 43 (3), pp. 649–664. Cited by: §II.
  • [4] A. Felner, R. Stern, S. E. Shimony, E. Boyarski, M. Goldenberg, G. Sharon, N. Sturtevant, G. Wagner, and P. Surynek (2017) Search-based optimal solvers for the multi-agent pathfinding problem: summary and challenges. In Tenth Annual Symposium on Combinatorial Search, Cited by: §II.
  • [5] C. R. Garrett, R. Chitnis, R. Holladay, B. Kim, T. Silver, L. P. Kaelbling, and T. Lozano-Pérez (2021) Integrated Task and Motion Planning. Annual review of control, robotics, and autonomous systems 4. Cited by: §II.
  • [6] I. Griva, S. G. Nash, and A. Sofer (2009) Linear and nonlinear optimization. Vol. 108, Siam. Cited by: §V-3.
  • [7] I. Gurobi Optimization (2021) Gurobi optimizer reference manual. URL http://www. gurobi. com. Cited by: §VII.
  • [8] V. N. Hartmann, A. Orthey, D. Driess, O. S. Oguz, and M. Toussaint (2021) Long-Horizon Multi-Robot Rearrangement Planning for Construction Assembly. arXiv preprint arXiv:2106.02489. Cited by: §II.
  • [9] K. Hauser and J. Latombe (2010) Multi-modal motion planning in non-expansive spaces. The International Journal of Robotics Research 29 (7), pp. 897–915. Cited by: §II, §IV.
  • [10] K. Hauser and V. Ng-Thow-Hing (2011) Randomized multi-modal motion planning for a humanoid robot manipulation task. The International Journal of Robotics Research 30 (6), pp. 678–698. Cited by: §II.
  • [11] W. Hönig, S. Kiesel, A. Tinka, J. Durham, and N. Ayanian (2018) Conflict-based search with optimal task assignment. In Proceedings of the International Joint Conference on Autonomous Agents and Multiagent Systems, Cited by: §II.
  • [12] W. Hönig, J. A. Preiss, T. S. Kumar, G. S. Sukhatme, and N. Ayanian (2018) Trajectory planning for quadrotor swarms. IEEE Transactions on Robotics 34 (4), pp. 856–869. Cited by: §IV-A.
  • [13] L. E. Kavraki, P. Svestka, J. Latombe, and M. H. Overmars (1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE transactions on Robotics and Automation 12 (4), pp. 566–580. Cited by: §IV.
  • [14] R. A. Knepper, T. Layton, J. Romanishin, and D. Rus (2013) Ikeabot: an autonomous multi-robot coordinated furniture assembly system. In 2013 IEEE International conference on robotics and automation, pp. 855–862. Cited by: §II.
  • [15] J. J. Kuffner and S. M. LaValle (2000) RRT-connect: an efficient approach to single-query path planning. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), Vol. 2, pp. 995–1001. Cited by: §IV.
  • [16] G. Laporte and I. H. Osman (1995) Routing problems: a bibliography. Annals of operations research 61 (1), pp. 227–262. Cited by: §V.
  • [17] O. S. Lawlor and L. V. Kalée (2002) A voxel-based parallel collision detection algorithm. In Proceedings of the 16th international conference on Supercomputing, pp. 285–293. Cited by: §VII.
  • [18] S. Li and J. A. Shah (2019) Safe and efficient high dimensional motion planning in space-time with time parameterized prediction. In 2019 International Conference on Robotics and Automation (ICRA), pp. 5012–5018. Cited by: §VII.
  • [19] H. Ma, D. Harabor, P. J. Stuckey, J. Li, and S. Koenig (2019) Searching with consistent prioritization for multi-agent path finding. In

    Proceedings of the AAAI Conference on Artificial Intelligence

    Vol. 33, pp. 7643–7650. Cited by: §II.
  • [20] L. Nägele, A. Hoffmann, A. Schierl, and W. Reif (2020) LegoBot: automated planning for coordinated multi-robot assembly of lego structures. In IEEE/RSJ Intl. Conf. on Intell. Robots and Systems, Cited by: §II.
  • [21] M. Phillips and M. Likhachev (2011) Sipp: safe interval path planning for dynamic environments. In 2011 IEEE International Conference on Robotics and Automation, pp. 5628–5635. Cited by: §VI.
  • [22] R. Shome and K. E. Bekris (2021) Synchronized multi-arm rearrangement guided by mode graphs with capacity constraints. In Algorithmic Foundations of Robotics XIV, pp. 243–260. External Links: ISBN 978-3-030-66723-8 Cited by: §II.
  • [23] I. Solis, J. Motes, R. Sandström, and N. M. Amato (2021) Representation-optimal multi-robot motion planning using conflict-based search. IEEE Robotics and Automation Letters 6 (3), pp. 4608–4615. Cited by: §II.
  • [24] M. Toussaint and M. Lopes (2017) Multi-bound tree search for logic-geometric programming in cooperative manipulation domains. In Proceedings - IEEE International Conference on Robotics and Automation, pp. 4044–4051. External Links: ISBN 9781509046331, Document, ISSN 10504729 Cited by: §II.
  • [25] H. Zhang, J. Chen, J. Li, B. C. Williams, and S. Koenig (2022) Multi-agent path finding for precedence-constrained goal sequences. In Proceedings of International Joint Conference on Autonomous Agents and Multi-agent Systems (**AAMAS 2022**), Cited by: §II, §VI.