RoboTSP – A Fast Solution to the Robotic Task Sequencing Problem  Francisco Su ́ arez-Ruiz, Teguh Santoso Lembono and Quang-Cuong Pham  Abstract — In many industrial robotics applications, such as spot-welding, spray-painting or drilling, the robot is required to visit successively multiple targets. The robot travel time among the targets is a significant component of the overall execution time. This travel time is in turn greatly affected by the order of visit of the targets, and by the robot configurations used to reach each target. Therefore, it is crucial to optimize these two elements, a problem known in the literature as the Robotic Task Sequencing Problem (RTSP). Our contribution in this paper is two-fold. First, we propose a fast, near-optimal, algorithm to solve RTSP. The key to our approach is to exploit the classical distinction between task space and configuration space, which, surprisingly, has been so far overlooked in the RTSP literature. Second, we provide an open-source implementation of the above algorithm, which has been carefully benchmarked to yield an efficient, ready-to-use, software solution. We discuss the relationship between RTSP and other Traveling Salesman Problem (TSP) variants, such as the Generalized Traveling Salesman Problem (GTSP), and show experimentally that our method finds motion sequences of the same quality but using several orders of magnitude less computation time than existing approaches.  I. I NTRODUCTION  In many industrial robotics applications, such as spot- welding, spray-painting or drilling, the robot is required to visit successively multiple targets. Consider for instance the drilling task depicted in Fig. 1, which was proposed at the Airbus Shopfloor Challenge held during ICRA 2016 in Stockholm, Sweden. The task, mimicking the actual drilling process in aircraft manufacturing, consisted in drilling as many holes as possible in one hour, from a given pattern of  245   hole positions. The robot travel time between the holes is a significant component of the overall execution time. This travel time is in turn greatly affected by the order of visit of the holes, and by the robot configurations used to reach each hole. Therefore, it is crucial to optimize these two elements, a problem known in the literature as the RTSP, see [1] for a recent review. Finally, note that, since the position of the panel is unknown at the beginning of the round, the planning time is included within the one hour limit. Thus, in this Challenge as in many practical applications, there is a need for an algorithm that can find near-optimal plans within minutes, not hours. RTSP is closely related to TSP, a classical problem in Computer Science. In TSP, a set of locations is given on a map, and one is interested in finding the order to visit all the locations while minimizing the total travel distance, see Fig. 2 (a). The key difference between RTSP and TSP is that, in RTSP, each of the targets (e.g. a hole position) may  The authors are with the School of Mechanical and Aerospace Engineer- ing, Nanyang Technological University, Singapore.  (a)   (b) Fig. 1.   The Airbus Shopfloor Challenge held during ICRA 2016. The task, mimicking the actual drilling process in aircraft manufacturing, consisted in drilling as many holes as possible, in one hour, from a given pattern of   245  hole positions. Since the position of the panel is unknown at the beginning of the round, the planning time is included within the one hour limit. a) The simulation environment. b) Our setup using a DENSO robot at the live Challenge, where our team won the second prize.  be reached by   multiple robot configurations , also known as  Inverse Kinematics (IK) solutions . One simple work-around may consist in assigning a fixed robot configuration for each target: RTSP then becomes a classical TSP among the assigned robot configurations. Such a work-around is however sub-optimal. Another approach consists in formulating RTSP as an instantiation of GTSP: in   GTSP,   the   locations   are   split   into   bins,   and   one   is required to visit exactly one location per bin, see Fig. 2 (b). Here, each bin will contain the different robot configurations corresponding to the same target. While there have been many works devoted to GTSP and some efficient solutions exist, the sheer size of real-world RTSP instances   1   make this approach inapplicable in practice. Section II provides a more detailed discussion on existing approaches to solve RTSP. Our   contribution   in   this   paper   is   two-fold.   First,   we propose a fast, near-optimal, algorithm to solve RTSP. The key to our approach is to exploit the classical   distinction between task space and configuration space , which, surpris- ingly, has been so far overlooked in the RTSP literature. Specifically, we propose a three-step algorithm [see Fig. 2 (c) for illustration]: 1) Find a (near-)optimal visit order of the targets in a  task-space metric   (e.g. Euclidean distance between the hole positions), using classical TSP algorithms; 2) Given the order found in Step 1, find for each target  1 For instance, in the set-up of the Airbus Shopfloor Challenge, using a discretization of   π  2   radians for the free-DoF, one obtains   3 ,   779   different configurations, grouped into   245   bins, which cannot be solved in practical times by any existing GTSP solver. See also Section IV-D for a detailed comparison.  arXiv:1709.09343v2 [cs.RO] 5 Oct 2017
(a)   (b) Configuration Space  Task Space  (c) Fig. 2.   (a) The Traveling Salesman Problem. (b) The Generalized Traveling Salesman Problem. (c) Decomposition of the Robotic Task Sequencing Problem into task-space and configuration-space.  the optimal robot configuration, so that the total path length through the configurations is minimized in a  configuration-space metric   (e.g. Euclidean distance be- tween the robot configurations – collisions are ignored at   this   stage),   using   a   graph   shortest   path   search algorithm; 3) Compute the final collision-free configuration-space trajectories by running classical motion planning algo- rithms (e.g. Rapidly-Exploring Random Tree (RRT)) through the robot configurations found in Step 2 and in the order given by Step 1. Our second contribution is to provide an open-source implementation   2   of the above algorithm. In particular, we carefully benchmark different key components of the algo- rithm (underlying task-space TSP solver, configuration-space metric, discretization step-size for the free-DoF), so as to come up with an efficient, ready-to-use, software solution. The remainder of the paper is organized as follows. In Section II, we discuss existing approaches to RTSP. In Section III, we introduce and describe in detail the proposed method. In Section IV, we present the experimental results, showing in particular that our method finds motion sequences of the same quality but using several orders of magnitude less computation time than existing approaches. Finally, in Section V, we conclude with a few remarks. II. R ELATED WORKS  A recent survey on strategies to solve RTSP can be found in [1]. We recall some of the main results below. In [2], the authors plan the motions for a 3-DoF robot to visit   6   targets, each of which can be reached with two  2 The open-source implementation of the proposed method is accessible at   www.github.com/crigroup/robotsp  different robot configurations. For this, they formulate a GTSP with   6   bins and   2   configurations per bin. They then convert the GTSP into a TSP, which can be solved efficiently. In [3], the authors consider a fruit picking task, which has up to   250   targets, but with only one robot configuration per target. The problem can then be directly formulated as a TSP, which the authors solved using the Nearest-Neighbor heuristic. In [4], the authors consider a RTSP with one configuration per target, which can then be formulated as a regular TSP. The emphasis here is on collision-free trajectories, which are difficult to find when the environment includes obstacles. First, the authors approximate the travel cost between config- urations as the Euclidean distance in the configuration space. Then, a minimum spanning tree is computed using Prim’s algorithm to find a near-optimal tour under the approximated cost. Next, collision-free paths are calculated given this near- optimal tour. The collision-free paths yield an updated travel cost, which is then used to iteratively refine the tour. This idea of computing first a good tour with a simple metric (without considering collisions) before computing collision- free trajectories is reused in our present work (but without the iterative refinement step). An extension of this work is proposed in [5]. Here the authors consider multiple robot configurations per target. Instead of a spanning tree, they compute a near-optimal  group spanning tree   [6]. On a task involving   50   targets with  5   configurations per target, a near-optimal solution could be found in   9 ,   600   s. Following a different approach, the authors of [7] pro- pose to use Genetic Algorithms (GAs) to solve GTSP. The optimization criteria is the task cycle time. On a task with 3-DoF and 6-DoF robots involving   50   targets, a near-optimal solution is found in   1 ,   800   s. The quality of the solution depends on several control parameters (related to the GA) and the number of iterations. This approach has been further extended to include collision-free path planning for 2D and 3D environments [8], [9]. Yet another approach consists in formulating RTSP as a multi-objective constraint optimization problem. In [10], a robotic spray painting task is considered. The optimization criteria is set to minimize the task planning and execution time while maximizing the painting quality. Three constraints were defined: process, resources and quality constraints. The multi-objective problem then is solved using the Depth-First Search (DFS) algorithm. On a task involving   8   targets, with  4   configurations per target, an optimal solution is found in  10 ,   000   s. The main limitation of all the works discussed above is the large computation time they require. In particular, none of these works could have been applied to the setting of the Airbus Shopfloor Challenge presented in Section I, which involves   245   targets, with tens of configurations per target, and which has to be solved within a few minutes – since the planning time is counted in the one hour limit of the challenge.
III. R OBO TSP   ALGORITHM  A. Setting  Consider   n   targets in the task-space. A tour in the task- space that visits each target exactly once is called a   task- space tour   3 . We first compute IK solutions for each target – using a suitable discretization for the free-DoFs if necessary. A tour in the configuration-space that starts from the robot “home” configuration, visits, for each target, exactly one IK solution associated with that target, and returns to the “home” configuration is called a   configuration-space tour . Our ob- jective   is   to   find   the   fastest   collision-free   configuration- space tour subject to the robot constraints (e.g. velocity and acceleration bounds). Let   m i   be the number of IK solutions found for tar- get   i . If we do not take into account obstacles, there are  ( n   −   1)!( ∏ n i =1   m i )   possible configuration-space tours (with straight paths) for this task. One cannot therefore expect to find the optimal sequence by brute force in practical times.  B. Proposed algorithm  As presented in Section I, our method consists in: 1) Finding a (near-)optimal task-space tour in a   task- space metric ; 2) Given the order found in Step 1, finding, for each target, the optimal robot configuration, so that the corresponding configuration-space tour has minimal length in a   configuration-space metric   – collisions are ignored at this stage; 3) Computing fast collision-free   configuration-space   tra- jectories by running classical motion planning algo- rithms (e.g. RRT-Connect with post-processing [11], [12]) through the configurations found in Step 2 and in the order given by Step 1. Implementation details and benchmarking results for Steps 1 and 3 are given in Section IV. Regarding Step 2, we first construct an undirected graph as depicted in Fig. 3. Specifically, the graph has   n   layers, each layer   i   contains   m i   vertices representing the   m i   IK solutions of target   i   (the targets are ordered according to Step 1), resulting in a total of   ∑ n i =1   m i   vertices. Next, for  i   ∈   [1 , . . . , n   −   1] , we add an edge between each vertex of layer   i   and each vertex of layer   i   + 1 , resulting in a total of   ∑ n − 1  i =1   m i m i +1   edges. Finally, we add two special vertices: “Start” and “Goal”, which are associated with the robot “home” configuration, and connected respectively to the   m 1   vertices of the first layer and the   m n   vertices of the last layer. The edge costs are computed according to a configuration- space metric: for instance, the cost for the edge joining vertices   q   and   q ′   can be given by the Euclidean distance in the configuration space  √∑ DoF  k =1   ( q k   −   q ′  k ) 2 . Section IV-B examines in detail how the choice of such metrics influences the quality of the final path. One can note here that the  3 Strictly speaking, a tour requires to return to the first target, so we are making a slight abuse of vocabulary here. . . .   . . .   . . .   . . .   . . .  Start  Target 1   Target 2   Target 3   Target   n   −   1   Target   n  Goal  . . .  Fig. 3.   Graph constructed for Step 2 of the algorithm. The targets are ordered according to Step 1. The shortest path (green lines) connecting the “Start” and “Goal” vertices will yield the optimal configuration-space tour that visits the targets in the order specified by Step 1.  metric should be fast to compute – in particular, collisions are ignored at this stage – since the costs must be computed for all   m 1   +   ∑ n i =1   m i m i +1   +   m n   graph edges. Finally, we run a graph search algorithm to find the shortest path between the “Start” and “Goal” vertices. By construction, any path between the “Start” and “Goal” ver- tices will visit exactly one vertex in each layer, in the order specified by Step 1. Conversely, for any choice of IK solutions for the   n   targets, there will be a path in the graph between the “Start” and “Goal” vertices and going through the vertices representing these IK solutions. Therefore, Step 2 will find the   true optimal   selection of IK solutions that min- imize the total cost, according to the specified configuration- space metric, given the order of the targets.  C. Complexity analysis  For Step 1, it is well-known that TSP is NP-complete, which means that finding the true optimal tour for   n   targets has in practice an exponential complexity. Many heuristics have been developed over the years to find near-optimal tours. For instance,   2-Opt   [13] and Lin-Kernighan-Helsgaun (LKH) [14] can find tours in practical times with an opti- mality gap bellow   5%   and   1%   respectively [15]. For Step 2, let   M   be an upper-bound of the number of IK solutions   m i   per target. The number of graph vertices is then   O ( nM   )   and the number of the graph edges is  O ( nM   2 ) . Since Dijkstra’s algorithm (with binary heap) has a complexity in   O ( | E |   log   | V   | )   where   | E |   and   | V   |   are respectively the number of edges and vertices, Step 2 has a complexity in   O ( nM   2   log ( nM   ) ) . For Step 3, one has to make   n   −   1   queries to the motion planner, yielding a complexity in   O ( n ) . However, as the constant in the   O ()   (average computation time per motion planning query) is large, the overall computation time is dominated by that of Step 3 in our setting. In general, the computation time of motion planning queries depends largely on the environment (obstacles), see [16], [17] for recent benchmarking results showing the CPU time required when planning practical robot motions. IV. E XPERIMENTS  This section evaluates the proposed method when applied to the drilling task shown in Fig. 1. Our system is formed
0   25   50   100   150   200   245  0 . 001  0 . 01  0 . 1  1  10  100  1 , 000  Number of targets  CPU time [s]  Exact  2-Opt  RNN (a) 0   25   50   100   150   200   245  0  5  10  15  Number of targets  Optimality Gap [%]   (b) Fig. 4.   Benchmarking results for three task-space TSP solvers. (a) CPU times for different number of targets. The Y axis is logarithmic (b) Task execution time for different number of targets. The sets of targets are selected randomly. The last sample considers all the   245   targets.  by a Denso VS060 6-DoF industrial manipulator equipped with a commercial off-the-shelf hand drill. All benchmarks were executed in a system with Intel   R ©   Core TM   i7 processor and 24 GB RAM, GeForce GTX 960M video card, running Ubuntu 16.04 (Xenial), 64 bits.  A. Benchmarking task-space TSP solvers  To solve the task-space TSP (Step 1 of our algorithm), one may use exact or near-optimal solvers. The choice depends on the trade-off between the available CPU time and the solution quality. Here we evaluate three TSP solvers. 1)   Exact : Constraint Integer Programming (CIP) can be used to find true optimal TSP tour [15]. Here, we used the SCIP Optimization Suite [18] to implement an exact solver; 2)   2-Opt : We re-implemented this simple, yet efficient, algorithm   to   find   a   near-optimal   solution   to   TSP. The algorithm iteratively improves an initial guess by repeatedly replacing pairs of edges that cross over [15]. 3)   RNN : We re-implemented this algorithm, which con- sists in iteratively selecting the nearest neighbor as the next target to visit. This process is repeated until all the targets are visited [15]. One can do several   restarts  from different initial targets, and choose the tour with the lowest cost from all the restarts. The drawback of this method is that it tends to corner itself, which requires long edges to get back to unvisited targets. Fig. 4 shows a benchmark of the three methods. We run the TSP solvers on task-space subsets of   25 ,   50 ,   100 ,   150 ,  200   random targets as well as on the total   245   targets. One can observe that the   2-Opt   solver yields high-quality tours (less than   5%   of sub-optimality) with low CPU time usage (less than   1   s). As for the   Exact   solver, it is not practical for more than   150   targets. Therefore, for all the subsequent experiments, we shall use   2-Opt   as our near-optimal task- space TSP solver.  B. Benchmarking configuration-space metrics  The   configuration-space metric   that defines the edge cost in Step 2 of our algorithm is the key component for the 25   50   100   150   200   245  0 . 1  1  10  100  Number of targets  CPU time [s]  (a) 25   50   100   150   200   245  20  40  60  80  Number of targets  Task Execution Time [s]  Euclidean  Max Joint Difference  Linear Interpolation   (b) Fig. 5.   Benchmarking results for three C-space metrics. (a) CPU times for different number of targets. The Y axis is logarithmic (b) Task execution time for different number of targets. The sets of targets are selected randomly. The last sample considers all the   245   targets.  overall performance of the method. Given two robot con- figurations   q   and   q ′ , the ideal cost of the edge   c ∗ ( q ,   q ′ )  is the duration of a time-optimal collision-free trajectory between them. However, since there are thousands of such edges, running full-fledged motion planning algorithms (with collision checks) for every edge would not be tractable. Therefore, one must consider approximate metrics, which should be fast to compute, yet give a good prediction of the corresponding time-optimal collision-free trajectory duration. Here we evaluate three such metrics. 1)   Weighted Euclidean joint distance : The cost   c ( q ,   q ′ )   is estimated as the weighted L 2   norm:  c ( q ,   q ′ ) :=  √ √ √ √   DoF ∑  k =1  w k ( q ′  k   −   q k ) 2   ,  where   w k   is a positive weight for joint   k . The weights are chosen in proportion to the maximum possible distance (Euclidean distance in the task-space) traveled by any point on the robot, when moving along the corresponding joint. Similar to [19], in our experiments this metric outperforms consistently the Euclidean joint distance. 2)   Maximum joint difference : The cost   c ( q ,   q ′ )   is esti- mated as follows:  c ( q ,   q ′ ) := max  k  ∣ ∣ ∣ ∣  ( q ′  k   −   q k )  ̇ q max  k  ∣ ∣ ∣ ∣   .  The intuition of this metric is to determine the maxi- mum joint displacement when “moving” from   q   to   q ′  by simply computing the joint difference,   ( q ′  k   −   q k ) , over the joint   k   velocity limit, for   k   ∈   [1 , . . . ,   DoF ] . Then the maximum value is used; 3)   Linear trajectory interpolation : the cost   c ( q ,   q ′ )   is given by the duration of a trajectory obtained by linear interpolation. It only requires to specify the positions,  q   and   q ′ , and guarantees continuity at the position level subject to the robot velocity and acceleration bounds. Moreover,   this   metric   does   not   consider   obstacles which greatly reduces its computing time.
Fig. 5 shows the benchmarking results for the three pro- posed configuration-space metrics. One can observe that the  Maximum joint difference   metric takes the lowest CPU time and yields task execution times comparable to, in some cases even better than, the   Euclidean   and   Linear Interpolation metrics . Therefore, for all the subsequent experiments, we shall use   Maximum joint difference   as our metric.  C. Benchmarking discretization step size for the free DoF  Many industrial tasks such as spot-welding, spay-painting or drilling involve less than 6 degrees of freedom. Therefore, a classical 6-DoF industrial robot has more joints than strictly required to execute such tasks. Specifically, the drilling task at hand involves 5 DoF, since the rotation   θ   about the drilling direction is irrelevant. One approach to tackle this redun- dancy can consist in setting a specific value for the irrelevant DoF: for instance   θ   ∈ { 0 ,   π  2   , π,   3 π  2   }   for a   π  2   discretization step size. For each of the discretized value of   θ , we then have a full 6-DoF IK problem. To solve the full 6-DoF IK, we next use OpenRAVE’s IKFast [20], which outputs all the IK solutions (here we have a “discrete” redundancy situation – think of the “elbow up” and “elbow down” configurations). We finally group all the IK solutions corresponding to all the discretized values of   θ   into a single list, which is the list of all IK solutions that will be considered for a given target. Table I gives the total number of IK solutions considered as a function of the discretization step size and of the number of targets.  TABLE I N UMBER OF ROBOT CONFIGURATIONS DEPENDING ON THE DISCRETIZATION STEP SIZE AND THE NUMBER OF TARGETS  Step   Number of targets Size   25   50   100   150   200   245  π   235   538   1 ,   044   1 ,   548   2 ,   071   2 ,   495  π  2   344   816   1 ,   598   2 ,   368   3 ,   094   3 ,   779  π  3   481   1170   2 ,   287   3 ,   362   4 ,   424   5 ,   418  π  4   623   1 ,   505   2 ,   939   4 ,   377   5 ,   668   6 ,   990  π  6   952   2 ,   236   4 ,   417   6 ,   528   8 ,   471   10 ,   421  π  12   1 ,   934   4 ,   428   8 ,   724   12 ,   981   16 ,   811   20 ,   720  One can see that the choice of the   discretization step size  is governed by a trade-off between speed and optimality. Fig. 6 shows the computation time and task execution time as a function of the discretization step size. As expected, the computation time increases as the discretization step size decreases, but interestingly, the task execution time does not change significantly for step sizes below   π  4   , which thus yields a good trade-off between CPU time and task execution time. Therefore, for all the subsequent experiments, we shall use  π  4   as our discretization step size.  D. Comparison to other methods  As none of the methods described in Section II provide public implementations, we were unable to reproduce their π   π  2  π  3  π  4  π  6  π  12  0  10  20  Discretization step size  CPU time [s]  n   = 25  n   = 50  n   = 100  n   = 150  n   = 200  n   = 245  (a) π   π  2  π  3  π  4  π  6  π  12  20  40  60  80  100  Discretization step size  Task Execution Time [s]   (b) Fig. 6.   Benchmarking results showing the effect of the discretization step size of the free DoF. 0   25   50   100   150   200   245  10  100  1 , 000  10 , 000  Number of targets  CPU time [s] 0   25   50   100   150   200   245  20  40  60  80  Number of targets  C-space Cost 0   25   50   100   150   200   245  50  100  Number of targets  Task Execution Time [s]   GLKH (iters = 1000)  GLKH (iters = 500)  GLKH (iters = 100)  TSP C-Space  RoboTSP  Fig. 7.   Comparison of our method ( RoboTSP ) against other methods: i) Solved as TSP in configuration-space; ii) Solved as GTSP using GLKH.  results and perform a fair comparison with the method herein presented. However, we can note that the computation times reported in previous works are several orders of magnitude higher than ours, yet for problem instances that are smaller   4  than what we consider in this work. In the following, we compare our method to two of the existing methods, which we could re-implement. 1)   TSP in C-space   [3]: when only one configuration is considered per target, RTSP is reduced to a regular TSP in the configuration space. Here, for each target, we consider the IK solution with the best manipulability index [21]; 2)   GLKH : here the RTSP is formulated as a GTSP [5], [22], [23]. To solve that GTSP, we use the state-of-the- art GLKH solver [24] which makes use of the LKH  4 The problem instance size is considered in terms of targets and config- urations per target
heuristic [14]. Fig. 7 shows the comparison of our method ( RoboTSP ) to the two methods just described. While the   TSP in C- space   method has a similar running time as   RoboTSP   (indeed both run a TSP on the same number of targets), the time durations of the trajectories it produces are higher than those of   RoboTSP , since it does not optimize the IK choice per target.  GLKH   produces trajectories with similar total duration as  RoboTSP   but the computing time is higher several orders of magnitude. A visualization with the trajectories produced by these three methods to visit all the   245   targets is available at  https://youtu.be/w33QfRjKFs8 . V. C ONCLUSIONS  We have proposed a method to determine a near-optimal sequence to visit   n   targets with multiple configurations per target, also known as the Robotic Task Sequencing Problem. For a complex drilling task, which requires visiting 245 targets with an average of 28.5 configurations per target, our method could compute a high-quality solution in less than a minute. To our knowledge, no existing approach could have solved the same problem in practical times. We have also provided a carefully benchmarked open-source software solution, which can be readily used in complex, real-world, applications such as drilling, spot-welding or spray-painting. A CKNOWLEDGMENT  This   work   was   supported   in   part   by   NTUitive   Gap Fund   NGF-2016-01-028   and   SMART   Innovation   Grant NG000074-ENG. R EFERENCES [1]   S. Alatartsev, S. Stellmacher, and F. Ortmeier, “Robotic Task Sequenc- ing Problem: A Survey,”   Journal of Intelligent & Robotic Systems , vol. 80, no. 2, pp. 279–298, 2015. [2]   L. L. Abdel-Malek and Z. Li, “The application of inverse kinematics in the optimum sequencing of robot tasks,”   International Journal of Production Research , vol. 28, no. 1, pp. 75–90, 1990. [3]   Y. Edan, T. Flash, U. Peiper, I. Shmulevich, and Y. Sarig, “Near- minimum-time task planning for fruit-picking robots,”   IEEE Transac- tions on Robotics and Automation , vol. 7, no. 1, pp. 48–56, 1991. [4]   M. Saha, G. Sanchez-Ante, and J.-C. Latombe, “Planning multi-goal tours for robot arms,” in   IEEE International Conference on Robotics and Automation , vol. 3.   IEEE, 2003, pp. 3797–3803. [5]   M. Saha, T. Roughgarden, J.-C. Latombe, and G. S ́ anchez-Ante, “Planning Tours of Robotic Arms among Partitioned Goals,”   The International Journal of Robotics Research , vol. 25, no. 3, pp. 207– 223, 2006. [6]   C. Chekuri, G. Even, and G. Kortsarz, “A greedy approximation algo- rithm for the group Steiner problem,”   Discrete Applied Mathematics , vol. 154, no. 1, pp. 15–34, 2006. [7]   P. Zacharia and N. Aspragathos, “Optimal robot task scheduling based on genetic algorithms,”   Robotics and Computer-Integrated Manufac- turing , vol. 21, no. 1, pp. 67–79, 2005. [8]   E. K. Xidias, P. T. Zacharia, and N. A. Aspragathos, “Time-optimal task scheduling for articulated manipulators in environments cluttered with obstacles,”   Robotica , vol. 28, no. 03, p. 427, may 2010. [9]   P. T. Zacharia, E. K. Xidias, and N. A. Aspragathos, “Task scheduling and motion planning for an industrial manipulator,”   Robotics and Computer-Integrated Manufacturing , vol. 29, no. 6, pp. 449–462, dec 2013. [10]   E. Kolakowska, S. F. Smith, and M. Kristiansen, “Constraint optimiza- tion model of a scheduling problem for a robotic arm in automatic systems,”   Robotics and Autonomous Systems , vol. 62, no. 2, pp. 267– 280, 2014. [11]   J. Kuffner and S. LaValle, “RRT-connect: An efficient approach to single-query path planning,” in   IEEE International Conference on Robotics and Automation (ICRA) , vol. 2, 2000, pp. 995–1001. [12]   Q.-C. Pham, “Trajectory Planning,” in   Handbook of Manufacturing Engineering and Technology .   London: Springer London, 2015, pp. 1873–1887. [13]   G. A. Croes, “A Method for Solving Traveling-Salesman Problems,”  Operations Research , vol. 6, no. 6, pp. 791–812, 1958. [14]   K. Helsgaun, “An effective implementation of the Lin-Kernighan trav- eling salesman heuristic,”   European Journal of Operational Research , vol. 126, no. 1, pp. 106–130, oct 2000. [15]   D. L. Applegate, R. E. Bixby, V. Chvatal, and W. J. Cook,   The Traveling Salesman Problem : a Computational Study.   Princeton University Press, 2011. [16]   I. A. Sucan, M. Moll, and L. E. Kavraki, “The Open Motion Planning Library,”   IEEE Robotics & Automation Magazine , vol. 19, no. 4, pp. 72–82, 2012. [17]   J. Meijer, Q. Lei, and M. Wisse, “Performance study of single-query motion planning for grasp execution using various manipulators,” in  2017 18th International Conference on Advanced Robotics (ICAR) . IEEE, 2017, pp. 450–457. [18]   T. Achterberg, “SCIP: solving constraint integer programs,”   Mathe- matical Programming Computation , vol. 1, no. 1, pp. 1–41, jul 2009. [19]   R. Bohlin and L. Kavraki, “Path planning using lazy PRM,” in   IEEE International Conference on Robotics and Automation (ICRA) .   IEEE, 2000, pp. 521–528. [20]   R. Diankov, “Automated Construction of Robotic Manipulation Pro- grams,”   Ph.D.   dissertation,   Carnegie   Mellon   University,   Robotics Institute, aug 2010. [21]   T. Yoshikawa, “Manipulability of Robotic Mechanisms,”   The Interna- tional Journal of Robotics Research , vol. 4, no. 2, pp. 3–9, jun 1985. [22]   C. Wurll, D. Henrich, and H. W ̈ orn, “Multi-Goal Path Planning for Industrial Robots,” in   International Conference on Robotics and Application , 1999. [23]   C. Wurll and D. Henrich, “Point-to-point and multi-goal path planning for industrial robots,”   Journal of Robotic Systems , vol. 18, no. 8, pp. 445–461, 2001. [24]   K. Helsgaun, “Solving the equality generalized traveling salesman problem using the Lin-Kernighan-Helsgaun Algorithm,”   Mathematical Programming Computation , vol. 7, no. 3, pp. 269–287, sep 2015.