A Constraint Programming Approach to Simultaneous Task Allocation and Motion Scheduling for Industrial Dual-Arm Manipulation Tasks

01/23/2019 ∙ by Jan Kristof Behrens, et al. ∙ Bosch 0

Modern lightweight dual-arm robots bring the physical capabilities to quickly take over tasks at typical industrial workplaces designed for workers. In times of mass-customization, low setup times including the instructing/specifying of new tasks are crucial to stay competitive. We propose a constraint programming approach to simultaneous task allocation and motion scheduling for such industrial manipulation and assembly tasks. The proposed approach covers dual-arm and even multi-arm robots as well as connected machines. The key concept are Ordered Visiting Constraints, a descriptive and extensible model to specify such tasks with their spatiotemporal requirements and task-specific combinatorial or ordering constraints. Our solver integrates such task models and robot motion models into constraint optimization problems and solves them efficiently using various heuristics to produce makespan-optimized robot programs. The proposed task model is robot independent and thus can easily be deployed to other robotic platforms. Flexibility and portability of our proposed model is validated through several experiments on different simulated robot platforms. We benchmarked our search strategy against a general-purpose heuristic. For large manipulation tasks with 200 objects, our solver implemented using Google's Operations Research tools and ROS requires less than a minute to compute usable plans.



There are no comments yet.


page 1

page 4

page 6

page 8

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

Modern lightweight dual-arm robots such as the ABB YuMi or the KaWaDa Nextage are engineered in the style of a human torso to be easily applicable in industrial workplaces designed for workers. These types of robots are an answer to the demand for flexible, cost-efficient production of customer-driven product variants and small lot sizes.

Such flexible production requires fast methods to specify new tasks for these robots. Classical teach-in by means of fixed poses and paths is not appropriate. With the capabilities of today’s perception systems, which can detect and localize workpieces, boxes, and tools automatically in typical workplaces, and a formalized goal or high-level task specification, the manual teach-in may be replaced by automated planning – in principle. Optimal planning involves three aspects: (a) task planning of the necessary steps and actions to achieve the overall goal/task, (b) scheduling of these steps and actions, and (c) motion planning for each step and action.

Fig. 1: Assembling of wiper motors with a dual-arm robot. The robot picks a tool from (C), places it on the shaft of the rotor of an electric motor in the workpiece holder (A), picks an electric interface, supplied in a container (B) and places it on (A).

Dual-arm manipulation further requires to decide about (d) the allocation of task steps and actions to the individual arms. Moreover, the complexity of scheduling and motion planning is increased heavily, due to the necessity to closely coordinate the manipulators to prevent self-collisions of the robot.

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

In many industrial use-cases, task planning is not required as the necessary steps and actions to process and assemble a workpiece are already given in digital form. That is, we already have an abstract plan, but with a number of unknowns and degrees of freedom in terms of the three aspects scheduling, allocation and motion planning. Computing an optimal, executable plan requires to treat these aspects in a highly integrated and coherent manner, which we refer to as

simultaneous task allocation and motion scheduling (STAAMS). An optimal plan depends not only on the motion of the manipulators but also on the order in which a workpiece is assembled, the order in which the components are taken from boxes or conveyor belts, in which they are processed by other machines, etc. – in particular, if connected systems or machines impose temporal constraints. The number of actions to be scheduled can be very high which results in big combinatorial complexity. Moreover, a suitable STAAMS solver has to consider different assignments of subtasks to arms, while taking the individual working ranges into account as well as task steps in which the arms have to cooperate.

In this paper, we propose a flexible model and solver for STAAMS for multi-arm robots in industrial use-cases. The proposed model and solver are based on constraint programming (CP) and constraint optimization, respectively. In detail, our contributions are as follows:

1) For specifying the abstract task decomposition of a STAAMS problem, we propose a novel and intuitive model primitive named Ordered Visiting Constraints (OVC). The OVC concept is developed out of the observation that many production steps can be described concisely by sequences of actions (e.g. drilling, picking, welding or joining) to be performed with one of the robot arms at given locations, with ordering or temporal constraints between them.

2) For the robot motions, we propose a model of time-scalable motion series that can be directly integrated with constraint-based scheduling, utilizing the fact that many industrial workplaces provide a controlled and unobstructed environment in which motion planning can be performed using precomputed roadmaps.

3) We propose an advanced CP concept named Connection Variables to link the two submodels – the OVC-based task model and the motion model – into an unified STAAMS problem model. Connection Variables are a special kind of CP meta variables on the indicies of other CP variables. At the same time, we explain how this modularity allows to port a given OVC-based task model to different workplace layouts and robots.

4) We present an adaptable solver, which allows for fine-grained user control over the different constraint optimization techniques to compute an almost-optimal plan 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 in Sec. II before we discuss related work in Sec. III. Our main contributions, the STAAMS model with the OVC-based task model and the motion model as well as the corresponding solver, are presented in the Sections IV and V, respectively. We show the scalability and portability 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 of characteristic properties and prevalent concepts. These properties and the concept of Ordered Visiting Actions serves as basis for the design of our STAAMS model in Section IV.

Ii-a Use-Cases

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

Injection molding. Parts have to be taken from a source container and inserted into an injection molding machine. When the molding process is finished, the parts have to be taken from the machine and placed under a camera for visual inspection and hold into a fixture for an electrical check. The latter requires to press a button simultaneously to start the check. While the molding machine may process two parts simultaneously, the visual and electrical checks can only process one part at a time. Finally, the finished parts are placed in another container. Full containers have to be placed for collection.

Ii-B Analysis

These industrial use-cases show several characteristic properties:

Controlled environment. Industrial workplaces provide by design a controlled and unobstructed environment. Therefore, we assume that all object locations and possible placements are known in advance, which allows for offline pre-calculation of motion roadmaps and collision tables. Furthermore, we may assume the absence of external interferences such as humans.

Unobstructed workspace. We assume that relevant objects never obstruct each other. This implies that there exists a collision-free subset of the workspace that does not alter over time and allows to reach all relevant object locations with at least one robot arm. For example this applies to drilling, riveting, welding, glueing, and assembling of small parts. As a consequence, we do not require a complex scene graph (cf. [3]) that tracks geometric relations between all objects in the workspace.

Ordered Visiting Actions. Suitable plans for these use-cases may be specified as a series of motions (per arm) to visit relevant locations in the workspace. At each location, the manipulator may perform local actions such as screw in a screw or picking an object from a container, which – for our scheduling purposes – can be abstracted as constraint on the visiting duration at that location. While the overall order of actions may be changed, some actions like pick-and-place are subject to a partial 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 model many advanced tasks such as joining, welding, sorting, inspecting, drilling, and milling.

Temporal dependencies. Often, there are additional temporal dependencies between OVAs. For example, molding has to precede the visual and electrical checks in the molding use-case and in the motor assembly use-case (as shown in Fig. 1) the temporal dependencies are given by the assembly sequence for each motor. In the sorting use-case, there are no temporal dependencies between the OVAs per se. Yet, each arm can transport only a single object at a time, i.e. the gripper is a reservable resource, which requires to schedule the OVAs per arm.

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

Ii-C STAAMS Problem Definition

Based on this analysis, STAAMS can be formulated as the anonymous variant of multi-agent path finding problem, combined with target assignments (TAPF) given by partially-ordered sets of subtasks/actions with spatiotemporal, combinatorial and ordering constraints on predefined locations (6DoF poses); and is NP-hard [21].

Iii Related Work

The state-of-the-art optimal TAPF method [20] cannot solve STAAMS problems in general, as it does not compute kinematically feasible motions for agents, nor can it be applied in cases requiring ordering decisions about task assignments. Online methods for multi-agent task assignment and scheduling algorithms have been developed for small-sized teams of agents, and highly flexible against execution uncertainty [24]. Multi-robot task allocation with temporal/ordering constraints has been studied in the context of integrating auction-based methods with Simple Temporal Problems [22]. These methods, however, do not account for conflicting spatial interactions, as needed, for example, in dual-arm manipulations. The applications of CP to multi-robot task planning and scheduling often use a simplified robot motion model, and ignore the cost of spatial interaction among robots in the scheduling process [4].

The motion planning and scheduling sub-problems of STAAMS can be seen as a multi-robot motion planning problem. State-of-the-art approaches tackling this problem often do not address the task/goal allocation problem, i.e., goals/tasks are assumed to be given. LaValle [17] formulates the motion scheduling problem in a joint configuration space. Prioritized planning assigns an order to the robots (arms) according to which their movements are planned (e.g., [15]). In fixed-path planning – also referred to as time-scaling – only timings are adjusted to prevent collisions [23]. In fixed-roadmap planning, topological graphs are used to both plan the paths and adjust the timings. Our proposed method falls in the third category. A fixed-roadmap is particularly suitable for 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 modeled easily as a special-case with our STAAMS model. Our approach performs such time-scaling in its last step. In Sec. VI, we compare our simultaneous task allocation and motion scheduling approach with pure time-scaling using an experimental setup in the style of the one used by Kimmel et al.

Alatartsev et al. [1] present a survey about the task sequencing problem for industrial robots, where sources for execution variants are systematically identified for a given task specification (e.g., multiple inverse kinematic solutions, partial ordering) and optimized based on various cost functions. The survey, however, lacks the coverage for tasks that are applicable for multi-arm robots. Kolakowska et al. [14] schedule paint strokes by ignoring the dependency between the ordering of the strokes and their motions. This approach is not generalizable to multi-robot scenarios, as this dependency cannot be ignored due to robot-robot collisions. Representing task orders can be done via hierarchical task networks (HTN) [9]. However, HTN would not by itself be capable of generating the orderings in the plan in a way that is optimized, or even feasible from the point of view of the robot’s geometry or motions.

In our approach, we employ CP to model the abstract task specification and the robot motion. Similarly, Ejenstam et al. [7] use CP to solve the problem of dual-arm manipulation planning and cell layout optimization via a coarse discretization of the workspace. Conversely, we create dense roadmaps to enable the close coordination of arms, thus allow more parallel movements of arms. Kurosu et al. [15] describe a decoupled MILP-based approach to solve a STAAMS, where the motion planner is prone to fail due to simplified motion and cost models used in the one-shot MILP formulation. This is not the case for us, as a single CP solver finds a mutually feasible solution for all sub-problems.

In our previous work [2], we hand-coded the full requirements of the robot and its workspace in the MiniZinc language. In this paper, we introduce a coherent formalism which allows to model the robot and workspace as 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 data objects such as the roadmaps for motion planning, which may be created automatically from 3D sensor data and cover the full workspace of a real robot. Most important, our planning system uses carefully chosen and evaluated variable ordering and value selection heuristics for efficient planning (see Fig. 5). For the implementation, we used the Google Operation Research tools [10], which (in contrast to MiniZinc solvers) allow fine-grained definition of the search strategy.

Iv Modeling STAAMS Problems with OVCs

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

In this section, we present our formalism for specifying STAAMS problems as constraint programs using OVCs. Our model consists of two submodels named task model and motion model. As illustrated in Figure 2, the task model is independent of any kinematic details and actual trajectories. Conversely, the motion model represents the trajectories of all active components independent of any task information. Both models are linked through Connection Variables, a special kind of CP variable explained below.

Next, we explain both submodels and then the Connection Variable mechanism. For readability, we write constants or values as lowercase Latin letters and constraint variables as capital letters. Compounds of constraint variables are denoted with small Greek letters.

Iv-a Task Model

The first and most important element of the task model are OVCs, which can be considered as variable, constraint-based blueprints of OVAs. An OVC consists of four sets of CP variables modeling primitive actions (e.g. pick, place, drill, etc.) to be executed at certain locations in the workplace within certain time intervals by an active component. The locations are a finite set of 6DoF poses of interest in the workplace – in particular possible object placements in containers and workpiece holders – in a common reference system.
Def 1. Formally, an OVC is a tuple

The variables represent the primitive actions, the variables describe the locations, and the variables model the time intervals. The variable represents the active component to be used. A triple , and denotes that active component shall perform action during time at location .

In , arbitrary constraints on and between these variables can be specified. In particular, each can be constrained to a specific primitive action to be executed. Similarly, each is typically constrained to one or few specific locations or specific location combinations for all location variables. Also, quantitative temporal constraints on the time intervals may be given.

The task model also allows for arbitrary constraints between OVCs, named inter-OVC constraints . Typical examples are temporal constraints between OVCs (e.g., for synchronization or ordering of OVCs or combinatorial constraints – e.g., to distribute locations amongst OVCs). In the following, we refer to the set of all OVCs as .

The second element of the task model are resources which describe abstract or physical objects such as tool, workpiece holders or robot grippers. A resource can be reserved exclusively for arbitrary time intervals. Typically, reservations are defined the by referencing start or end variables of interval variables of those OVCs that require this resource.

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

Iv-B Motion Model

The motion model represents the trajectories of all active components – independent of any task information – as motion series consisting of configuration variables – in the respective joint space of the active component – with time interval variables for the transition in-between. To be able to model the motion with CP, a roadmap-based approach is used (cf. for example [11]). The roadmap of an active component is a sufficiently dense sampling of the joint space, where the nodes are joint configurations and the edges represent short collision-free motions between them (cf. Fig. 3). In particular, the roadmap contains one or more nodes for each location in reach of the active component.
Def 2. Hence, the motion series of an active component is a sequence of configuration variables and a sequence of interval variables

where the domain of the variables are the nodes of the active component’s roadmap . An interval variable models the time spent at configuration whereas denotes the traveling time between the configurations and . For this purpose, the roadmap edges denote the expected traveling time as edge weight. The roadmap thus yields information about paths between configurations and their durations to be used in the CP. Note that the roadmaps may also include multiple nodes for the same configuration – for example to consider the different collision geometries of the arm depending on the gripper state. The set of all motion series – one for each active component – is named . Collisions between any two active components and are prevented by a constraint requiring that pairs of conflicting joint configurations and , which are precomputed in a collision table, must not be assumed simultaneously.

Iv-C Connection Variables

The task model does not contain any information about the actual joint configurations and thus motions of the active components. Conversely, the motion model has no information on the OVCs, primitive actions, locations and resources. Yet, the two submodels are linked in two ways: First, for each active component, the location mapping links the active component’s roadmap nodes with the locations. That is, for each location , it provides a set of the joint configurations that reach . Second, the Connection Variables link from the location variables of the task model to configuration variables of the motion model. Such a connection denotes that the configuration variable has to be chosen such that the respective active component reaches the location given in the location variable. There exists exactly one such connection per location variable. Configuration variables not referenced by Connection Variables are used for evasive movements to avoid collisions and deadlocks.

The key idea is that these connections are also CP variables. Therefore the name Connection Variables. They can be considered as meta variables, as they specify to which configuration variable to point to. Hence, formally, the domain of the Connection Variable for the location variable of OVC is the index of the configurations of the motion series of the active component of . We refer to this mechanism as index-based. An assignment states that the th configuration variable of the motion series of has to reach to the th location of , formally . In this way, the Connection Variables establish the execution order for the OVCs assigned to an active component.

Connection Variables of always have to be strictly monotonic, i.e. , since the locations of have to be visited in this order. Yet, two OVCs for the same active component may be interleaved (e.g., , , , and ) if there is no conflicting inter-OVC constraint or resource-constraint. Two Connection Variables must never reference the same configuration variable.

Iv-D Examples for Task Modeling with OVCs

In the following, we exemplary define two parts of the molding use-case. First, we model the electrical check which 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, two actions have to be coordinated. This task is modeled by two OVCs: for holding the part into the fixture and to push the button for starting the check. has one location variable constrained to the button location (i.e. ). has three location variables constrained to the pick location of the object, the fixture location, and the destination container. To synchronize the two OVCs, we constrain the push interval ( of ) to be during the fixture interval ( of ).

To model the bi-manual pick up of a workpiece container in the molding use-case, we create two OVCs and , each of length . We constrain the first location variables to assume either of the locations }. Through an inter-OVC constraint, we ensure that the combination of selected grasp poses yields a valid combination for a stable grasp. We constrain the second location variables to take either of the locations } and an additional inter-OVC constraint ensures, that all pick and place locations take compatible values. Temporal constraints ensure that the first intervals end together and the second intervals start together. Note, that the arms have to be controlled by a dedicated controller for the actual carrying. We assume, that the controller either provides information about occupied space over time, such that the solver can schedule possible other components to not interfere, or stays within a given subset of the workspace.

V STAAMS Solver for OVCs

In the following, we explain the horizon estimation for the number of configurations in each motion series. Then, we describe the overall search strategy, detailing the choices for variable and value selection heuristics.

V-a Horizon estimation

Initially, we do not know the optimal horizon for every active component, i.e. the number of configuration variables. Therefore, we integrate an iterative deepening approach directly in our model. For each active component , we create a constraint variable named horizon. We prevent any movements after the th configuration in the motion series of by constraining all configurations to . Small horizon values generally render the problem unsatisfiable, while large values bloat the search space unnecessarily and may cause superfluous motions. A lower bound for is the number of all location variables of the OVCs assigned to the corresponding active components. Therefore, the placement of the horizon variables in the search strategy is important, which is explained in the following Section.

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

V-B Search strategy

A constraint satisfaction solver computes one or more variable assignments that each satisfy all constraints. Such solvers usually interleave a backtracking search with constraint propagation. In the backtracking search, variables are selected according to a variable-ordering heuristic, and values for the variables are chosen based on a value-ordering heuristic.

Variable ordering. The Connection Variables constitute a special case in our model. Due to their index-based mechanism, the constraint information from the motion model to the task model and vice-versa cannot be propagated until decisions on the involved Connection Variables have been made. The Connection Variables, again, require to first decide on the active component variables and the location variables . In our use-cases, searching (1.) on the location variables, (2.) on the active component variables, (3.) on the Connection Variables, (4.) on the horizon variables, and then (5.) on the configurations variables of the motion series yielded reliably good solutions within a few seconds planning time. This order makes sure, that all constraints are added to the model before any time is spent on the actual motion scheduling. However, our approach allows the user to customize or further refine the search strategy. This includes the behavior within those five variable batches or the overall order, which is further discussed in Section VI. At this stage, only the the time interval variables of the resources, OVCs and motion series remain to be decided. As the time interval variables of the resources are connected to the OVCs, which in turn are linked with the motion series by the Connection Variables, the solver has to decide about the time-scaling of the motion series. More precisely, the solver has to decide about the waiting times of each motion series. The time-scaling allows to prevent collisions, resolve resource conflicts, and satisfy any inter-OVC constraint (e.g. synchronization or ordering). Also, no superfluous waiting times should be added to optimize the makespan. By solving this time-scaling problem as final step, we obtain the timed motion plans for all active components.

Value selection. For each selected variable, the solver has to assign a value from the variable’s domain. In case of the Connection Variables (4) and horizon variables (3), we use a minimum value heuristic to foster short motion series. For (1), (2), and (5), we use a random value selection heuristic as there is no clear preference for these variables. In case of a good value selection, the remaining search process involves only few backtracks. We employ a Luby restart strategy (cf. [19]) to avoid long-lasting searches in the time-scaling step (i.e. in the 6th step). This is especially useful when poor value selections have been made in steps (1) to (5).

Fig. 5: Aggregated plot over experiments showing the influence of the search strategy on the solution quality after for different problem sizes.

Vi Evaluation

We implemented our STAAMS model and solver in Python using the Google Operation Research Tools [10] and experimented with a KaWaDa Nextage dual-arm robot in a Gazebo simulation environment [13] on a HP zBook laptop. We implemented the first and second use-case given in Sec. II-A. Here, we focus on the sorting use-case, which resembles the experiment by Kimmel at al. for their dual-arm coordination algorithm [12], and compare the results. Afterwards, we show how our solver scales on instances of this use-case for up to objects. The modularity of our STAAMS model (cf. Fig. 2) enables the re-use of tasks expressed as Ordered Visiting Constraints. To show this, we deployed an example task (taking all objects from a table) on two robotic platforms by reusing the task model.

Comparison with pure time-scaling. We modeled three instances of the sorting use case with increasing number of objects from   to   and varying degree of conflict between the two arms (see Fig. 4). Then, we compared our approach against the theoretical lower bound obtained by ignoring collisions between the manipulators as well as against the method by Kimmel et al., which time-scales the trajectories of both manipulators to prevent collisions. We mimic their solver by using a randomized but fixed order of collecting the objects and leave only the scheduling to our solver. The results are visualized in Fig. 4. The diagrams show plots of the makespan (as quality measure) over the time spent to solve the instance (stopped after ) for ten different fixed order runs (red) and ten runs with order optimization (dark-blue). Our solver produces the first solutions sometimes as fast as in and usually converges within on the instances shown. By optimizing the order, our solver consistently outperforms the fixed order runs – or reaches the same performance in the rare case that by chance a very good order is selected. Since both approaches utilize some random decisions, the plotted outcomes visualize a distribution. With this in mind, it becomes very clear that our STAAMS solver provides much more consistent and higher-quality results. In scenario (b), it gets very close to the theoretical lower bound (light blue). Interestingly, it takes only more to handle eight additional objects in (b) compared to (a).

Problem size [parts]

Norm. Makespan

First Solution

Budget: 10sec

Budget: 30sec

Budget: 45sec

Budget: 60sec

Best Solution

Fig. 6: In this diagram, the solution quality (makespan) divided by the lower bound (which ignores collisions between the manipulators) is shown for different problem sizes and different stages in the search. The vertical lines in the right half of the figure indicate cases in which no solution was found within the budgets of or rather .

Scalability. To evaluate the scaling properties of our approach, we ran a series of experiments similar to scenario (b) with a time limit of . Starting from the twenty parts depicted in Fig. 4b, we added for each experiment two extra parts to the scene – one for each arm – up to parts in total. In Fig. 6, the normalized makespan, i.e. the makespan divided by the theoretical lower bound, is plotted over the problem size for the first solutions, the best solutions, and computing time budgets from to . The solution quality for the first solution ranges approximately from to normalized makespan (solutions with a normalized makespan of approx. can be constructed), which rapidly improves with the following solutions to finally settle around normalized makespan.

Our solver computes high-quality solutions even for large problem instances in a few seconds or tens of seconds. Please note that the high scalability compared to ITAMP planners stems from two facts: First, STAAMS solving does not require to decide about the actions to be executed but rather to complete and optimize a given abstract plan (here modeled by OVCs) only. Second, in our motion model we limit the motions to stick to predefined roadmaps.

Custom search strategies. In Fig. 5, we compared four strategies, which are compliant with the rules explained in Sec. V, with a general-purpose baseline strategy. For the baseline strategy, variables were selected using a minimum domain heuristic, while values were selected randomly. We ran a total of 125 experiments for each. With each strategy, we solved five differently sized problems ( OVCs). Each of these experiments were executed five times with varied random seed for the solver. Fig. 7 shows how the five strategies perform over time on a problem with OVCs – it can be seen that the strategy makes a significant difference for the convergence speed. With these experiments we show, that although solutions can be found with the baseline strategy, the specific search strategies are necessary to achieve good solutions in acceptable solving time. In our use-cases, all custom strategies delivered comparable performance after of search (see Fig. 7). However, the differences for short solving times clearly show the benefit of employing a suitable search strategy.

Portability. Flexibility and portability of our modeling language are validated through several experiments on different simulated robot platforms (see Fig. 8). The task models, i.e. the sets of OVCs, that have been used to perform the pallet emptying on the KaWaDa Nextage and KUKA LBR iiwa platforms are identical. The differences are:111The code and the setup details are available at https://github.com/boschresearch/STAAMS-SOLVER The robot model (Moveit! [25] robot configuration to access motion planning, kinematic calculations, and collision checking); a seed robot configuration (as required by the Inverse kinematics (ik) solvers); a ”tuck” robot configuration (in which the arms do not obstruct each others workspaces); the names of kinematic chains, end-effectors, and the base frame; the static scene collision layout (represented in meshes or primitive shapes; and the locations of the workpieces. With this information and scripts in place, our system automatically creates the roadmaps, collision tables, and name-location-configuration mappings.

Fig. 7: Makespan vs. planning time for different search strategies on a OVC task.

Vii Conclusion and Future Work

In this paper, we proposed a flexible model and solver for simultaneous task allocation and motion scheduling (STAAMS) based on constraint programming (CP) and constraint optimization for industrial manipulation and assembly tasks for dual-arm robots. The core modeling concepts are Ordered Visiting Constraints and time-scalable motion series, which are linked by meta CP variables named Connection Variables. In our evaluation, we showed that our STAAMS solver quickly completes and optimizes a given problem model instance – i.e. an abstract task specifications given as collection of OVCs for a robot motion model – and delivers high-quality, executable motion plans. We demonstrated the scalability of our approach on large problem instances with up to actions, which were solved in less than . We also showed, that the OVC concept allows to transfer a given task model to another robot and/or workspace by exchanging the relevant motion submodels only.

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

We assume that our task-centric robot programming approach is suited not only for textual specification but also for multi-modal input variants. Therefore, we want to explore robot programming by natural language and demonstrations.

To broaden the applicability of this approach, we plan to include more task primitives (additionally to reaching goals) like trajectories for welding. We will also investigate the extension of our approach to include action models with safe approximations, when the actual space occupancy and duration are not known, e.g., when employing force-position control.

Viii Acknoledgement

The authors thank the project Robotics for Industry 4.0 (reg. no. CZ.02.1.01/0.0/0.0/15 003/0000470) for providing the KUKA robot simulation setup used in the Evaluation section.


  • [1] Sergey Alatartsev, Sebastian Stellmacher, and Frank Ortmeier. Robotic Task Sequencing Problem: A Survey. J Intell Robot Syst, 80(2):279–298, November 2015.
  • [2] Jan Kristof Behrens, Ralph Lange, and Michael Beetz. CSP-Based integrated Task & Motion Planning for Assembly Robots. In Proc. of the Workshop on AI Planning and Robotics at ICRA ’17, Singapore, May 2017.
  • [3] S. Blumenthal, H. Bruyninckx, W. Nowak, and E. Prassler. A scene graph based shared 3D world model for robotic applications. In Proc. of ICRA ’13, pages 453–460, Karlsruhe, Germany, May 2013.
  • [4] Kyle E. C. Booth, Goldie Nejat, and J. C. Beck. A constraint programming approach to multi-robot task allocation and scheduling in retirement homes. In Proc. of Principles and Practice of Constraint Programming (CP), 2016.
  • [5] S. Cambon, R. Alami, and F. Gravot. A Hybrid Approach to Intricate Motion, Manipulation and Task Planning. Int’l Journal of Robotics Research, 28(1):104–126, January 2009.
  • [6] Neil T Dantam, Zachary K Kingston, Swarat Chaudhuri, and Lydia E Kavraki. An incremental constraint-based framework for task and motion planning. The International Journal of Robotics Research, March 2018.
  • [7] Joakim Ejenstam. Implementing a Time Optimal Task Sequence For Robot Assembly Using Constraint. Master’s thesis, Uppsala Universitet, Sweden, 2014.
  • [8] Caelan Reed Garrett, Tomas Lozano-Perez, and Leslie Pack Kaelbling. FFRob: Leveraging Symbolic Planning for Efficient Task and Motion Planning. The International Journal of Robotics Research, 37(1):104–136, January 2018.
  • [9] Malik Ghallab, Dana Nau, and Paolo Traverso. Automated Planning and Acting. Cambridge University Press, New York, NY, USA, 1st edition, 2016.
  • [10] Google Inc. Google Optimization Tools. Retrieved February 28, 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-dimensional configuration spaces. IEEE Trans. on Robotics and Automation, 12(4):566–580, August 1996.
  • [12] A. Kimmel and K. E. Bekris. Scheduling Pick-and-Place Tasks for Dual-arm Manipulators using Incremental Search on Coordination Diagrams. In Proc. of PlanRob Workshop at ICAPS ’16, London, UK, June 2016.
  • [13] N. Koenig and A. Howard. Design and use paradigms for Gazebo, an open-source multi-robot simulator. In 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), volume 3, September 2004.
  • [14] Ewa Kolakowska, Stephen F. Smith, and Morten Kristiansen. Constraint optimization model of a scheduling problem for a robotic arm in automatic systems. Robotics and Autonomous Systems, 62(2):267–280, February 2014.
  • [15] Jun Kurosu, Ayanori Yorozu, and Masaki Takahashi. Simultaneous Dual-Arm Motion Planning for Minimizing Operation Time. Applied Sciences, 7(12):1210, November 2017.
  • [16] F. Lagriffoul and B. Andres. Combining task and motion planning: A culprit detection problem. Int’l Journal of Robotics Research, 35(8):890–927, July 2016.
  • [17] S. M. LaValle. Planning Algorithms. Cambridge University Press, 2006.
  • [18] T. Lozano-Pérez and L. P. Kaelbling. A constraint-based method for solving sequential manipulation planning problems. In Proc. of IROS ’14, pages 3684–3691, September 2014.
  • [19] M. Luby, A. Sinclair, and D. Zuckerman. Optimal speedup of Las Vegas algorithms. Information Processing Letters, 47(4):173–180, September 1993.
  • [20] Hang Ma and Sven Koenig. Optimal target assignment and path 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, Wolfgang Hönig, TK Kumar, Tansel Uras, Hong Xu, Craig Tovey, and Guni Sharon. Overview: Generalizations of multi-agent path finding to real-world scenarios. In Proc. of the IJCAI-16 Workshop on Multi-Agent Path Finding, 2016.
  • [22] E. Nunes, M. Manner, H. Mitiche, and M. Gini. A taxonomy for task allocation problems with temporal and ordering constraints. Robotics and Autonomous Systems, 2017.
  • [23] P. A. O’Donnell and T. Lozano-Perez. Deadlock-free and collision-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 task assignment and scheduling. In Proc of ICAPS, 2009.
  • [25] Ioan A. Sucan and Sachin Chitta. Moveit! Retrieved September 08, 2018, from http://moveit.ros.org.