A Constraint Programming Approach to Simultaneous TaskAllocation and Motion Scheduling for Industrial Dual-Arm

Manipulation Tasks

Jan Kristof Behrens1 Ralph Lange1 Masoumeh Mansouri2

Abstract— Modern lightweight dual-arm robotsbring the physical capabilities to quickly take overtasks at typical industrial workplaces designed forworkers. In times of mass-customization, low setuptimes including the instructing/specifying of newtasks are crucial to stay competitive. We proposea constraint programming approach to simultaneoustask allocation and motion scheduling for such indus-trial manipulation and assembly tasks. The proposedapproach covers dual-arm and even multi-arm robotsas well as connected machines. The key concept areOrdered Visiting Constraints, a descriptive and exten-sible model to specify such tasks with their spatiotem-poral requirements and task-specific combinatorialor ordering constraints. Our solver integrates suchtask models and robot motion models into constraintoptimization problems and solves them efficiently us-ing various heuristics to produce makespan-optimizedrobot programs. The proposed task model is robotindependent and thus can easily be deployed to otherrobotic platforms. Flexibility and portability of ourproposed model is validated through several exper-iments on different simulated robot platforms. Webenchmarked our search strategy against a general-purpose heuristic. For large manipulation tasks with200 objects, our solver implemented using Google’sOperations Research tools and ROS requires less thana minute to compute usable plans.

I. IntroductionModern lightweight dual-arm robots such as the ABBYuMi or the KaWaDa Nextage are engineered in thestyle of a human torso to be easily applicable in in-dustrial workplaces designed for workers. These types ofrobots are an answer to the demand for flexible, cost-efficient production of customer-driven product variantsand small lot sizes.

Such flexible production requires fast methods to spec-ify new tasks for these robots. Classical teach-in bymeans of fixed poses and paths is not appropriate. Withthe capabilities of today’s perception systems, which candetect and localize workpieces, boxes, and tools auto-matically in typical workplaces, and a formalized goal orhigh-level task specification, the manual teach-in may bereplaced by automated planning – in principle. Optimalplanning involves three aspects: (a) task planning ofthe necessary steps and actions to achieve the overallgoal/task, (b) scheduling of these steps and actions, and

1Robert Bosch GmbH, Corporate Research, Renningen, Ger-many, behrens.jk@gmail.com, ralph.lange@de.bosch.com

2Orebro University, Sweden, masoumeh.mansouri@oru.se

Fig. 1. Assembling of wiper motors with a dual-arm robot. Therobot picks a tool from (C), places it on the shaft of the rotorof an electric motor in the workpiece holder (A), picks an electricinterface, supplied in a container (B) and places it on (A).(c) motion planning for each step and action. Dual-armmanipulation further requires to decide about (d) theallocation of task steps and actions to the individualarms. Moreover, the complexity of scheduling and mo-tion planning is increased heavily, due to the necessityto closely coordinate the manipulators to prevent self-collisions of the robot.

All four aspects – task planning, scheduling, allocationand motion planning – are closely interrelated. Ideally,to achieve optimal plans with regard to the makespan(production time) or similar objectives, they have tobe considered in one coherent formalism and planningalgorithm. In the last years, significant progress hasbeen made to closely couple task planning with motionplanning by passing feedback from motion planning totask planning (e.g., [8], [5], [16], [18], [6]), but research isstill far from an ideal solution.

In many industrial use-cases, task planning is notrequired as the necessary steps and actions to processand assemble a workpiece are already given in digitalform. That is, we already have an abstract plan, butwith a number of unknowns and degrees of freedom interms of the three aspects scheduling, allocation andmotion planning. Computing an optimal, executable planrequires to treat these aspects in a highly integrated andcoherent manner, which we refer to as simultaneous taskallocation and motion scheduling (STAAMS). An optimalplan depends not only on the motion of the manipulatorsbut also on the order in which a workpiece is assembled,the order in which the components are taken from boxesor conveyor belts, in which they are processed by othermachines, etc. – in particular, if connected systems or

arX

iv:1

901.

0791

4v1

[cs

.RO

] 2

3 Ja

n 20

19

machines impose temporal constraints. The number ofactions to be scheduled can be very high which resultsin big combinatorial complexity. Moreover, a suitableSTAAMS solver has to consider different assignments ofsubtasks to arms, while taking the individual workingranges into account as well as task steps in which thearms have to cooperate.

In this paper, we propose a flexible model and solverfor STAAMS for multi-arm robots in industrial use-cases.The proposed model and solver are based on constraintprogramming (CP) and constraint optimization, respec-tively. In detail, our contributions are as follows:1) For specifying the abstract task decomposition ofa STAAMS problem, we propose a novel and intuitivemodel primitive named Ordered Visiting Constraints(OVC). The OVC concept is developed out of the ob-servation that many production steps can be describedconcisely by sequences of actions (e.g. drilling, picking,welding or joining) to be performed with one of therobot arms at given locations, with ordering or temporalconstraints between them.2) For the robot motions, we propose a model of time-scalable motion series that can be directly integratedwith constraint-based scheduling, utilizing the fact thatmany industrial workplaces provide a controlled andunobstructed environment in which motion planning canbe performed using precomputed roadmaps.3) We propose an advanced CP concept named Con-nection Variables to link the two submodels – the OVC-based task model and the motion model – into an unifiedSTAAMS problem model. Connection Variables are aspecial kind of CP meta variables on the indicies of otherCP variables. At the same time, we explain how thismodularity allows to port a given OVC-based task modelto different workplace layouts and robots.4) We present an adaptable solver, which allows forfine-grained user control over the different constraintoptimization techniques to compute an almost-optimalplan for typical STAAMS problem sizes in few seconds.

The remainder of this paper is organized as follows:We present an analysis of typical industrial use-cases inSec. II before we discuss related work in Sec. III. Ourmain contributions, the STAAMS model with the OVC-based task model and the motion model as well as thecorresponding solver, are presented in the Sections IVand V, respectively. We show the scalability and portabil-ity of the proposed system and compare it to pure time-scaling in Sec. VI. The paper is concluded in Sec. VII.

II. Use-case Analysis and Problem Definition

In this section, we describe two typical industrial use-cases for dual-arm robots, followed by an analysis ofcharacteristic properties and prevalent concepts. Theseproperties and the concept of Ordered Visiting Actionsserves as basis for the design of our STAAMS model inSection IV.

A. Use-CasesSorting objects. The robot has to pick up colored

objects from the table and place them depending ontheir color in one of two containers. All parts on thetable are reachable by both arms. The containers are onlyreachable by either of the arms (see Fig. 4) so that anobject’s color defines the arm that has to pick this object.This use-case inspired by Kimmel et al. [12] will serve asa reference use-case for the evaluation.

Injection molding. Parts have to be taken from asource container and inserted into an injection moldingmachine. When the molding process is finished, the partshave to be taken from the machine and placed under acamera for visual inspection and hold into a fixture foran electrical check. The latter requires to press a buttonsimultaneously to start the check. While the moldingmachine may process two parts simultaneously, the visualand electrical checks can only process one part at atime. Finally, the finished parts are placed in anothercontainer. Full containers have to be placed for collection.

B. AnalysisThese industrial use-cases show several characteristicproperties:

Controlled environment. Industrial workplacesprovide by design a controlled and unobstructed envi-ronment. Therefore, we assume that all object locationsand possible placements are known in advance, whichallows for offline pre-calculation of motion roadmaps andcollision tables. Furthermore, we may assume the absenceof external interferences such as humans.

Unobstructed workspace. We assume that relevantobjects never obstruct each other. This implies that thereexists a collision-free subset of the workspace that doesnot alter over time and allows to reach all relevantobject locations with at least one robot arm. For examplethis applies to drilling, riveting, welding, glueing, andassembling of small parts. As a consequence, we donot require a complex scene graph (cf. [3]) that tracksgeometric relations between all objects in the workspace.

Ordered Visiting Actions. Suitable plans for theseuse-cases may be specified as a series of motions (perarm) to visit relevant locations in the workspace. Ateach location, the manipulator may perform local actionssuch as screw in a screw or picking an object from acontainer, which – for our scheduling purposes – canbe abstracted as constraint on the visiting duration atthat location. While the overall order of actions may bechanged, some actions like pick-and-place are subject to apartial ordering and are therefore considered as an entity.We refer to such entities as Ordered Visiting Actions(OVA) in the following. OVAs may be used to modelmany advanced tasks such as joining, welding, sorting,inspecting, drilling, and milling.

Temporal dependencies. Often, there are addi-tional temporal dependencies between OVAs. For ex-ample, molding has to precede the visual and electrical

checks in the molding use-case and in the motor assemblyuse-case (as shown in Fig. 1) the temporal dependenciesare given by the assembly sequence for each motor. Inthe sorting use-case, there are no temporal dependenciesbetween the OVAs per se. Yet, each arm can transportonly a single object at a time, i.e. the gripper is areservable resource, which requires to schedule the OVAsper arm.

Active components. Another important observationis that processing stations in the workspace may alsotake on different configurations, just as the robot arms.An example is the door of the molding machine in thesecond use-case. We refer to such stations and all activerobot components together as active components.

C. STAAMS Problem DefinitionBased on this analysis, STAAMS can be formulated asthe anonymous variant of multi-agent path finding prob-lem, combined with target assignments (TAPF) givenby partially-ordered sets of subtasks/actions with spa-tiotemporal, combinatorial and ordering constraints onpredefined locations (6DoF poses); and is NP-hard [21].

III. Related Work

The state-of-the-art optimal TAPF method [20] cannotsolve STAAMS problems in general, as it does notcompute kinematically feasible motions for agents, norcan it be applied in cases requiring ordering decisionsabout task assignments. Online methods for multi-agenttask assignment and scheduling algorithms have beendeveloped for small-sized teams of agents, and highlyflexible against execution uncertainty [24]. Multi-robottask allocation with temporal/ordering constraints hasbeen studied in the context of integrating auction-basedmethods with Simple Temporal Problems [22]. Thesemethods, however, do not account for conflicting spa-tial interactions, as needed, for example, in dual-armmanipulations. The applications of CP to multi-robottask planning and scheduling often use a simplified robotmotion model, and ignore the cost of spatial interactionamong robots in the scheduling process [4].

The motion planning and scheduling sub-problems ofSTAAMS can be seen as a multi-robot motion plan-ning problem. State-of-the-art approaches tackling thisproblem often do not address the task/goal allocationproblem, i.e., goals/tasks are assumed to be given.LaValle [17] formulates the motion scheduling problem ina joint configuration space. Prioritized planning assignsan order to the robots (arms) according to which theirmovements are planned (e.g., [15]). In fixed-path planning– also referred to as time-scaling – only timings areadjusted to prevent collisions [23]. In fixed-roadmap plan-ning, topological graphs are used to both plan the pathsand adjust the timings. Our proposed method falls in thethird category. A fixed-roadmap is particularly suitablefor industrial settings, as the environments are not often

subject to change. Kimmel et al. [12] employ a time-scaling approach to schedule two given sequences of pick-and-place tasks. Time-scaling problems can be modeledeasily as a special-case with our STAAMS model. Ourapproach performs such time-scaling in its last step. InSec. VI, we compare our simultaneous task allocationand motion scheduling approach with pure time-scalingusing an experimental setup in the style of the one usedby Kimmel et al.

Alatartsev et al. [1] present a survey about the tasksequencing problem for industrial robots, where sourcesfor execution variants are systematically identified fora given task specification (e.g., multiple inverse kine-matic solutions, partial ordering) and optimized basedon various cost functions. The survey, however, lacksthe coverage for tasks that are applicable for multi-armrobots. Kolakowska et al. [14] schedule paint strokes byignoring the dependency between the ordering of thestrokes and their motions. This approach is not generaliz-able to multi-robot scenarios, as this dependency cannotbe ignored due to robot-robot collisions. Representingtask orders can be done via hierarchical task networks(HTN) [9]. However, HTN would not by itself be capableof generating the orderings in the plan in a way that isoptimized, or even feasible from the point of view of therobot’s geometry or motions.

In our approach, we employ CP to model the abstracttask specification and the robot motion. Similarly, Ejen-stam et al. [7] use CP to solve the problem of dual-armmanipulation planning and cell layout optimization viaa coarse discretization of the workspace. Conversely, wecreate dense roadmaps to enable the close coordinationof arms, thus allow more parallel movements of arms.Kurosu et al. [15] describe a decoupled MILP-basedapproach to solve a STAAMS, where the motion planneris prone to fail due to simplified motion and cost modelsused in the one-shot MILP formulation. This is not thecase for us, as a single CP solver finds a mutually feasiblesolution for all sub-problems.

In our previous work [2], we hand-coded the full re-quirements of the robot and its workspace in the MiniZ-inc language. In this paper, we introduce a coherent for-malism which allows to model the robot and workspaceas well as an abstract task plan and its invariants.We propose OVCs as task model primitives and time-scalable motion series as motion model primitives. Also,we provide automated procedures to create the dataobjects such as the roadmaps for motion planning, whichmay be created automatically from 3D sensor data andcover the full workspace of a real robot. Most important,our planning system uses carefully chosen and evaluatedvariable ordering and value selection heuristics for effi-cient planning (see Fig. 5). For the implementation, weused the Google Operation Research tools [10], which (incontrast to MiniZinc solvers) allow fine-grained definitionof the search strategy.

𝐼𝐼1𝑤𝑤 𝐼𝐼2𝑤𝑤 𝐼𝐼3𝑤𝑤𝐼𝐼1𝑡𝑡 𝐼𝐼2𝑡𝑡 ⋯ 𝐼𝐼𝑛𝑛𝑡𝑡

C1 Cn

OVC ω2

MotionModel

TaskModel

ConnectionVariables

resource r1

I5I1

⋯ X5X1

L5⋯

OVC ω1

I3I1

A

⋯ X3X1

L1 ⋯

makespan

L1L3

𝐼𝐼1𝑤𝑤 𝐼𝐼2𝑤𝑤 𝐼𝐼3𝑤𝑤𝐼𝐼1𝑡𝑡 𝐼𝐼2𝑡𝑡 ⋯ 𝐼𝐼𝑚𝑚𝑡𝑡

C1 C2 C3 Cm

= integer variable = interval variable = index-based assignment

A

motion series σ2

motion series σ1

Fig. 2. Overview of our CP-based STAAMS model

IV. Modeling STAAMS Problems with OVCsIn this section, we present our formalism for specifyingSTAAMS problems as constraint programs using OVCs.Our model consists of two submodels named task modeland motion model. As illustrated in Figure 2, the taskmodel is independent of any kinematic details and actualtrajectories. Conversely, the motion model represents thetrajectories of all active components independent of anytask information. Both models are linked through Con-nection Variables, a special kind of CP variable explainedbelow.

Next, we explain both submodels and then the Con-nection Variable mechanism. For readability, we writeconstants or values as lowercase Latin letters and con-straint variables as capital letters. Compounds of con-straint variables are denoted with small Greek letters.

A. Task ModelThe first and most important element of the taskmodel are OVCs, which can be considered as variable,constraint-based blueprints of OVAs. An OVC consistsof four sets of CP variables modeling primitive actions(e.g. pick, place, drill, etc.) to be executed at certainlocations in the workplace within certain time intervalsby an active component. The locations L are a finite setof 6DoF poses of interest in the workplace – in particularpossible object placements in containers and workpieceholders – in a common reference system.Def 1. Formally, an OVC is a tuple

ω = (A, [P1, ..., Pl], [L1, ..., Ll], [I1, ..., Il], Cintra).The variables Pj represent the primitive actions, thevariables Lj ∈ L describe the locations, and the variablesIj model the time intervals. The variable A representsthe active component to be used. A triple Pj , Lj and Ij

denotes that active component A shall perform action Pj

during time Ij at location Lj .In Cintra, arbitrary constraints on and between these

variables can be specified. In particular, each Pj can beconstrained to a specific primitive action to be executed.Similarly, each Lj is typically constrained to one orfew specific locations or specific location combinationsfor all location variables. Also, quantitative temporalconstraints on the time intervals may be given.

The task model also allows for arbitrary constraintsbetween OVCs, named inter-OVC constraints Cinter.

Fig. 3. A roadmap for the left arm of a KaWaDa Nextage robot.

Typical examples are temporal constraints betweenOVCs (e.g., for synchronization or ordering of OVCs orcombinatorial constraints – e.g., to distribute m locationsamongst n OVCs). In the following, we refer to the setof all OVCs as Ω.

The second element of the task model are resourceswhich describe abstract or physical objects such as tool,workpiece holders or robot grippers. A resource r canbe reserved exclusively for arbitrary time intervals. Typ-ically, reservations are defined the by referencing startor end variables of interval variables of those OVCs thatrequire this resource.

B. Motion ModelThe motion model represents the trajectories of all activecomponents – independent of any task information – asmotion series consisting of configuration variables – inthe respective joint space of the active component – withtime interval variables for the transition in-between. Tobe able to model the motion with CP, a roadmap-basedapproach is used (cf. for example [11]). The roadmapof an active component is a sufficiently dense samplingof the joint space, where the nodes are joint configura-tions and the edges represent short collision-free motionsbetween them (cf. Fig. 3). In particular, the roadmapcontains one or more nodes for each location l ∈ L inreach of the active component.Def 2. Hence, the motion series σa of an activecomponent a is a sequence of m configuration variablesand a sequence of 2m− 1 interval variables

σa = ([C1, . . . , Cm], [Iw1 , I

t1, I

w2 , I

t2, . . . , I

wm]),

where the domain of the variables Ci are the nodesC of the active component’s roadmap r = (C,E). Aninterval variable Iw

i models the time spent at configura-tion Ci whereas It

i denotes the traveling time betweenthe configurations Ci and Ci+1. For this purpose, theroadmap edges E denote the expected traveling time asedge weight. The roadmap thus yields information aboutpaths between configurations and their durations to beused in the CP. Note that the roadmaps may also includemultiple nodes for the same configuration – for exampleto consider the different collision geometries of the armdepending on the gripper state. The set of all motionseries – one for each active component – is named Σ.Collisions between any two active components ai andaj are prevented by a constraint requiring that pairs

of conflicting joint configurations ci and cj , which areprecomputed in a collision table, must not be assumedsimultaneously.

C. Connection VariablesThe task model does not contain any information aboutthe actual joint configurations and thus motions of theactive components. Conversely, the motion model has noinformation on the OVCs, primitive actions, locationsand resources. Yet, the two submodels are linked in twoways: First, for each active component, the location map-ping links the active component’s roadmap nodes C withthe locations. That is, for each location l ∈ L, it providesa set of the joint configurations cl ⊂ C that reach l.Second, the Connection Variables link from the locationvariables of the task model to configuration variables ofthe motion model. Such a connection denotes that theconfiguration variable has to be chosen such that therespective active component reaches the location givenin the location variable. There exists exactly one suchconnection per location variable. Configuration variablesnot referenced by Connection Variables are used forevasive movements to avoid collisions and deadlocks.

The key idea is that these connections are also CP vari-ables. Therefore the name Connection Variables. Theycan be considered as meta variables, as they specify towhich configuration variable to point to. Hence, formally,the domain of the Connection Variable Xω,j for thelocation variable Lj of OVC ω is the index of theconfigurations [C1, . . . , Cm] of the motion series σ of theactive component A of ω. We refer to this mechanismas index-based. An assignment Xω,j = i states that theith configuration variable Ci of the motion series σ ofA has to reach to the jth location of ω, formally Ci ∈λ(Lj). In this way, the Connection Variables establishthe execution order for the OVCs assigned to an activecomponent.

Connection Variables of ω always have to be strictlymonotonic, i.e. Xω,j < Xω,j+1, since the locations[L1, . . . , Ll] of ω have to be visited in this order. Yet, twoOVCs for the same active component may be interleaved(e.g., Xω1,1 = 3, Xω2,1 = 4, Xω2,2 = 5, and Xω1,2 = 6) ifthere is no conflicting inter-OVC constraint or resource-constraint. Two Connection Variables must never refer-ence the same configuration variable.

D. Examples for Task Modeling with OVCsIn the following, we exemplary define two parts of the

molding use-case. First, we model the electrical checkwhich needs synchronized behavior of both arms. Second,we sketch a bi-manual pick of a workpiece container.

For the electrical check in the molding use-case, twoactions have to be coordinated. This task is modeled bytwo OVCs: ωfixture for holding the part into the fixtureand ωpush to push the button for starting the check.ωpush has one location variable constrained to the buttonlocation (i.e. L1 = lButton). ωfixture has three location

variables constrained to the pick location of the object,the fixture location, and the destination container. Tosynchronize the two OVCs, we constrain the push inter-val (I1 of ωpush) to be during the fixture interval (I2 ofωfixture).

To model the bi-manual pick up of a workpiece con-tainer in the molding use-case, we create two OVCsωL and ωR, each of length 2. We constrain the firstlocation variables L1 to assume either of the locationslGrasp1, lGrasp2. Through an inter-OVC constraint, weensure that the combination of selected grasp poses yieldsa valid combination for a stable grasp. We constrainthe second location variables L2 to take either of thelocations lPlace1, lPlace2 and an additional inter-OVCconstraint ensures, that all 4 pick and place locationstake compatible values. Temporal constraints ensure thatthe first intervals I1 end together and the second in-tervals I2 start together. Note, that the arms have tobe controlled by a dedicated controller for the actualcarrying. We assume, that the controller either providesinformation about occupied space over time, such thatthe solver can schedule possible other components to notinterfere, or stays within a given subset of the workspace.

V. STAAMS Solver for OVCsIn the following, we explain the horizon estimation forthe number of configurations in each motion series. Then,we describe the overall search strategy, detailing thechoices for variable and value selection heuristics.

A. Horizon estimationInitially, we do not know the optimal horizon m for everyactive component, i.e. the number of configuration vari-ables. Therefore, we integrate an iterative deepening ap-proach directly in our model. For each active componenta, we create a constraint variable H named horizon. Weprevent any movements after the Hth configuration inthe motion series of a by constraining all configurationsCi>H to CH . Small horizon values generally render theproblem unsatisfiable, while large values bloat the searchspace unnecessarily and may cause superfluous motions.A lower bound for H is the number of all location vari-ables of the OVCs assigned to the corresponding activecomponents. Therefore, the placement of the horizonvariables in the search strategy is important, which isexplained in the following Section.

B. Search strategyA constraint satisfaction solver computes one or morevariable assignments that each satisfy all constraints.Such solvers usually interleave a backtracking searchwith constraint propagation. In the backtracking search,variables are selected according to a variable-orderingheuristic, and values for the variables are chosen basedon a value-ordering heuristic.

Variable ordering. The Connection Variables con-stitute a special case in our model. Due to their index-based mechanism, the constraint information from the

.

.

.

a

.

.

b

.

.

c

.

100

.

101

.

102

.Planning Time [s] (log)

.

40

.

50

.

Makespa

n[s]

.

100

.

101

.

102

.Planning Time [s] (log)

.

55

.

60

.

65

.

70

.

100

.

101

.

102

.Planning Time [s] (log)

.

80

.

100

.

120

.

Random Fixed Order

.

Optimized Order

.

Ignoring Collisions

Fig. 4. Sorting scenarios (a)-(c) and makespan-vs-planning-time plots. Red lines show the makespan over planning time for a randomfixed order of execution (cf. [12]). The blue lines depict the makespan, when we let the solver decide on the order. A lower bound for eachproblem – obtained by ignoring collisions (relaxation of the problem) – is plotted in light blue, Blue Objects are dropped into a containerby the left arm at the left destination (green), and vice versa for the red objects. (a) 12 objects with high conflict potential, (b) as (a) butwith eight uncritical objects more to allow for efficient scheduling. (c) A randomly chosen instance with 24 objects and much interaction

motion model to the task model and vice-versa cannotbe propagated until decisions on the involved ConnectionVariables have been made. The Connection Variables,again, require to first decide on the active componentvariables A and the location variables Li. In our use-cases, searching (1.) on the location variables, (2.) onthe active component variables, (3.) on the ConnectionVariables, (4.) on the horizon variables, and then (5.) onthe configurations variables of the motion series yieldedreliably good solutions within a few seconds planningtime. This order makes sure, that all constraints areadded to the model before any time is spent on the actualmotion scheduling. However, our approach allows theuser to customize or further refine the search strategy.This includes the behavior within those five variablebatches or the overall order, which is further discussed inSection VI. At this stage, only the the time interval vari-ables of the resources, OVCs and motion series remain tobe decided. As the time interval variables of the resourcesare connected to the OVCs, which in turn are linked withthe motion series by the Connection Variables, the solverhas to decide about the time-scaling of the motion series.More precisely, the solver has to decide about the waitingtimes Iw

1 , . . . , IwH of each motion series. The time-scaling

allows to prevent collisions, resolve resource conflicts, andsatisfy any inter-OVC constraint (e.g. synchronization orordering). Also, no superfluous waiting times should beadded to optimize the makespan. By solving this time-scaling problem as final step, we obtain the timed motionplans for all active components.

Value selection. For each selected variable, the solverhas to assign a value from the variable’s domain. In caseof the Connection Variables (4) and horizon variables(3), we use a minimum value heuristic to foster shortmotion series. For (1), (2), and (5), we use a random valueselection heuristic as there is no clear preference for thesevariables. In case of a good value selection, the remainingsearch process involves only few backtracks. We employa Luby restart strategy (cf. [19]) to avoid long-lastingsearches in the time-scaling step (i.e. in the 6th step).

5 10 15 20 25Problem Size [parts]

500

1000

1500

2000

2500

3000

3500

4000

Aver

age

Mak

espa

n [s

]

MinDomComponent MinDomComponent RandomLocation MinDomLocation Random

Fig. 5. Aggregated plot over 125 experiments showing the influenceof the search strategy on the solution quality after 100 s for differentproblem sizes.

This is especially useful when poor value selections havebeen made in steps (1) to (5).

VI. EvaluationWe implemented our STAAMS model and solver inPython using the Google Operation Research Tools [10]and experimented with a KaWaDa Nextage dual-armrobot in a Gazebo simulation environment [13] on a HPzBook laptop. We implemented the first and second use-case given in Sec. II-A. Here, we focus on the sortinguse-case, which resembles the experiment by Kimmel atal. for their dual-arm coordination algorithm [12], andcompare the results. Afterwards, we show how our solverscales on instances of this use-case for up to 200 objects.The modularity of our STAAMS model (cf. Fig. 2)enables the re-use of tasks expressed as Ordered VisitingConstraints. To show this, we deployed an example task(taking all objects from a table) on two robotic platformsby reusing the task model.

Comparison with pure time-scaling. We modeledthree instances of the sorting use case with increasingnumber of objects from 12 to 24 and varying degree ofconflict between the two arms (see Fig. 4). Then, we com-pared our approach against the theoretical lower boundobtained by ignoring collisions between the manipula-tors as well as against the method by Kimmel et al.,which time-scales the trajectories of both manipulatorsto prevent collisions. We mimic their solver by usinga randomized but fixed order of collecting the objects

20 50 100 150 200Problem size [parts]

1.0

1.1

1.2

1.3

1.4N

orm

.Mak

espa

nFirst SolutionBudget: 10secBudget: 30secBudget: 45secBudget: 60secBest Solution

Fig. 6. In this diagram, the solution quality (makespan) dividedby the lower bound (which ignores collisions between the manipu-lators) is shown for different problem sizes and different stages inthe search. The vertical lines in the right half of the figure indicatecases in which no solution was found within the budgets of 10 orrather 30 sec.and leave only the scheduling to our solver. The resultsare visualized in Fig. 4. The diagrams show plots of themakespan (as quality measure) over the time spent tosolve the instance (stopped after 100 s) for ten differentfixed order runs (red) and ten runs with order optimiza-tion (dark-blue). Our solver produces the first solutionssometimes as fast as in 0.1 s and usually converges within3 s on the instances shown. By optimizing the order, oursolver consistently outperforms the fixed order runs –or reaches the same performance in the rare case thatby chance a very good order is selected. Since bothapproaches utilize some random decisions, the plottedoutcomes visualize a distribution. With this in mind, itbecomes very clear that our STAAMS solver providesmuch more consistent and higher-quality results. In sce-nario (b), it gets very close to the theoretical lower bound(light blue). Interestingly, it takes only 7 s more to handleeight additional objects in (b) compared to (a).

Scalability. To evaluate the scaling properties of ourapproach, we ran a series of 80 experiments similar toscenario (b) with a time limit of 180 s. Starting fromthe twenty parts depicted in Fig. 4b, we added for eachexperiment two extra parts to the scene – one for eacharm – up to 200 parts in total. In Fig. 6, the normalizedmakespan, i.e. the makespan divided by the theoreticallower bound, is plotted over the problem size for thefirst solutions, the best solutions, and computing timebudgets from 10 to 60 s. The solution quality for the firstsolution ranges approximately from 1.1 to 1.37 normal-ized makespan (solutions with a normalized makespan ofapprox. 2 can be constructed), which rapidly improveswith the following solutions to finally settle around 1.1normalized makespan.

Our solver computes high-quality solutions even forlarge problem instances in a few seconds or tens of sec-onds. Please note that the high scalability compared toITAMP planners stems from two facts: First, STAAMSsolving does not require to decide about the actions tobe executed but rather to complete and optimize a givenabstract plan (here modeled by OVCs) only. Second,in our motion model we limit the motions to stick topredefined roadmaps.

101 102

Planning Time [s] (log)

220

240

260

280

300

320

340

360

Mak

espa

n [s

]

Location MinDomLocation RandomMinDomComponent RandomComponent MinDom

Fig. 7. Makespan vs. planning time for different search strategieson a 25 OVC task.

Custom search strategies. In Fig. 5, we comparedfour strategies, which are compliant with the rules ex-plained in Sec. V, with a general-purpose baseline strat-egy. For the baseline strategy, variables were selectedusing a minimum domain heuristic, while values wereselected randomly. We ran a total of 125 experiments for100 s each. With each strategy, we solved five differentlysized problems (5−25 OVCs). Each of these experimentswere executed five times with varied random seed forthe solver. Fig. 7 shows how the five strategies performover time on a problem with 25 OVCs – it can be seenthat the strategy makes a significant difference for theconvergence speed. With these experiments we show,that although solutions can be found with the baselinestrategy, the specific search strategies are necessary toachieve good solutions in acceptable solving time. In ouruse-cases, all custom strategies delivered comparable per-formance after 20−30 s of search (see Fig. 7). However,the differences for short solving times clearly show thebenefit of employing a suitable search strategy.

Portability. Flexibility and portability of our model-ing language are validated through several experimentson different simulated robot platforms (see Fig. 8). Thetask models, i.e. the sets of OVCs, that have beenused to perform the pallet emptying on the KaWaDaNextage and KUKA LBR iiwa platforms are identical.The differences are:1 The robot model (Moveit! [25]robot configuration to access motion planning, kinematiccalculations, and collision checking); a seed robot con-figuration (as required by the Inverse kinematics (ik)solvers); a ”tuck” robot configuration (in which the armsdo not obstruct each others workspaces); the names ofkinematic chains, end-effectors, and the base frame; thestatic scene collision layout (represented in meshes orprimitive shapes; and the locations of the workpieces.With this information and scripts in place, our systemautomatically creates the roadmaps, collision tables, andname-location-configuration mappings.

VII. Conclusion and Future WorkIn this paper, we proposed a flexible model and solverfor simultaneous task allocation and motion scheduling

1The code and the setup details are available athttps://github.com/boschresearch/STAAMS-SOLVER

Fig. 8. A task – cleaning up the table – deployed on a KaWaDaNextage robot (left) and a pair of KUKA LBR iiwa robots (right).

(STAAMS) based on constraint programming (CP) andconstraint optimization for industrial manipulation andassembly tasks for dual-arm robots. The core modelingconcepts are Ordered Visiting Constraints and time-scalable motion series, which are linked by meta CPvariables named Connection Variables. In our evaluation,we showed that our STAAMS solver quickly completesand optimizes a given problem model instance – i.e.an abstract task specifications given as collection ofOVCs for a robot motion model – and delivers high-quality, executable motion plans. We demonstrated thescalability of our approach on large problem instanceswith up to 200 actions, which were solved in less than180 s. We also showed, that the OVC concept allowsto transfer a given task model to another robot and/orworkspace by exchanging the relevant motion submodelsonly. We assume that our task-centric robot program-ming approach is suited not only for textual specificationbut also for multi-modal input variants. Therefore, wewant to explore robot programming by natural languageand demonstrations.

To broaden the applicability of this approach, weplan to include more task primitives (additionally toreaching goals) like trajectories for welding. We willalso investigate the extension of our approach to includeaction models with safe approximations, when the actualspace occupancy and duration are not known, e.g., whenemploying force-position control.

VIII. Acknoledgement

The authors thank the project Robotics for Industry4.0 (reg. no. CZ.02.1.01/0.0/0.0/15 003/0000470) forproviding the KUKA robot simulation setup used in theEvaluation section.

References

[1] Sergey Alatartsev, Sebastian Stellmacher, and Frank Ort-meier. Robotic Task Sequencing Problem: A Survey. J IntellRobot Syst, 80(2):279–298, November 2015.

[2] Jan Kristof Behrens, Ralph Lange, and Michael Beetz. CSP-Based integrated Task & Motion Planning for AssemblyRobots. In Proc. of the Workshop on AI Planning and Roboticsat ICRA ’17, Singapore, May 2017.

[3] S. Blumenthal, H. Bruyninckx, W. Nowak, and E. Prassler.A scene graph based shared 3D world model for roboticapplications. In Proc. of ICRA ’13, pages 453–460, Karlsruhe,Germany, May 2013.

[4] Kyle E. C. Booth, Goldie Nejat, and J. C. Beck. A constraintprogramming approach to multi-robot task allocation andscheduling in retirement homes. In Proc. of Principles andPractice of Constraint Programming (CP), 2016.

[5] S. Cambon, R. Alami, and F. Gravot. A Hybrid Approachto Intricate Motion, Manipulation and Task Planning. Int’lJournal of Robotics Research, 28(1):104–126, January 2009.

[6] Neil T Dantam, Zachary K Kingston, Swarat Chaudhuri, andLydia E Kavraki. An incremental constraint-based frameworkfor task and motion planning. The International Journal ofRobotics Research, March 2018.

[7] Joakim Ejenstam. Implementing a Time Optimal Task Se-quence For Robot Assembly Using Constraint. Master’s thesis,Uppsala Universitet, Sweden, 2014.

[8] Caelan Reed Garrett, Tomas Lozano-Perez, and Leslie PackKaelbling. FFRob: Leveraging Symbolic Planning for EfficientTask and Motion Planning. The International Journal ofRobotics Research, 37(1):104–136, January 2018.

[9] Malik Ghallab, Dana Nau, and Paolo Traverso. AutomatedPlanning and Acting. Cambridge University Press, New York,NY, USA, 1st edition, 2016.

[10] Google Inc. Google Optimization Tools. Retrieved February28, 2018, from https://github.com/google/or-tools.

[11] L. E. Kavraki, P. Svestka, J. C. Latombe, and M. H. Overmars.Probabilistic roadmaps for path planning in high-dimensionalconfiguration spaces. IEEE Trans. on Robotics and Automa-tion, 12(4):566–580, August 1996.

[12] A. Kimmel and K. E. Bekris. Scheduling Pick-and-PlaceTasks for Dual-arm Manipulators using Incremental Searchon Coordination Diagrams. In Proc. of PlanRob Workshopat ICAPS ’16, London, UK, June 2016.

[13] N. Koenig and A. Howard. Design and use paradigmsfor Gazebo, an open-source multi-robot simulator. In 2004IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS), volume 3, September 2004.

[14] Ewa Kolakowska, Stephen F. Smith, and Morten Kristiansen.Constraint optimization model of a scheduling problem for arobotic arm in automatic systems. Robotics and AutonomousSystems, 62(2):267–280, February 2014.

[15] Jun Kurosu, Ayanori Yorozu, and Masaki Takahashi. Simulta-neous Dual-Arm Motion Planning for Minimizing OperationTime. Applied Sciences, 7(12):1210, November 2017.

[16] F. Lagriffoul and B. Andres. Combining task and motion plan-ning: A culprit detection problem. Int’l Journal of RoboticsResearch, 35(8):890–927, July 2016.

[17] S. M. LaValle. Planning Algorithms. Cambridge UniversityPress, 2006.

[18] T. Lozano-Perez and L. P. Kaelbling. A constraint-basedmethod for solving sequential manipulation planning prob-lems. In Proc. of IROS ’14, pages 3684–3691, September 2014.

[19] M. Luby, A. Sinclair, and D. Zuckerman. Optimal speedupof Las Vegas algorithms. Information Processing Letters,47(4):173–180, September 1993.

[20] Hang Ma and Sven Koenig. Optimal target assignment andpath finding for teams of agents. In Proc. of AAMAS ’16,AAMAS ’16, pages 1144–1152, 2016.

[21] Hang Ma, Sven Koenig, Nora Ayanian, Liron Cohen, WolfgangHonig, TK Kumar, Tansel Uras, Hong Xu, Craig Tovey, andGuni Sharon. Overview: Generalizations of multi-agent pathfinding to real-world scenarios. In Proc. of the IJCAI-16Workshop on Multi-Agent Path Finding, 2016.

[22] E. Nunes, M. Manner, H. Mitiche, and M. Gini. A taxon-omy for task allocation problems with temporal and orderingconstraints. Robotics and Autonomous Systems, 2017.

[23] P. A. O’Donnell and T. Lozano-Perez. Deadlock-free andcollision-free coordination of two robot manipulators. In Proc.of ICRA ’89, pages 484–489, Tsukuba, Japan, May 1989.

[24] Julie A. Shah, Patrick R. Conrad, and Brian Charles Williams.Fast distributed multi-agent plan execution with dynamic taskassignment and scheduling. In Proc of ICAPS, 2009.

[25] Ioan A. Sucan and Sachin Chitta. Moveit! Retrieved Septem-ber 08, 2018, from http://moveit.ros.org.