I Introduction
It is well known that the general motion planning problem is PSPACEcomplete [1] and that the runtime of even the best known exact algorithm is exponential in the dimension of the configuration space [2]. While samplingbased motion planning algorithms such as rapidly exploring random trees (RRTs) [3] and probabilistic roadmaps (PRMs) [4] are able to avoid explicit reconstruction of the free configuration space Canny’s algorithm [2]
relies upon, they cannot avoid the curse of dimensionality. In particular, Esposito’s Conditional Density Growth Model (CDGM) for RRTs
[5], predicts that the expected number of samples required for an RRT to explore a certain volume fraction of a space remains exponential in the dimension. For this reason, even though modern samplingbased planners have exhibited dramatic improvements over the original RRT and PRM techniques, computing real time solutions for systems with 10 or more degrees of freedom, such as hyperredundant manipulators, snakelike robots or humanoids, remains a significant challenge.This study focuses on this path planning problem in highdimensional configuration spaces. The intuition arises from three observations:

For many systems, it is rare for all of the kinematic abilities to be needed for a specific task [6].
Based on this intuition, we explore an algorithmic enhancement aimed at reducing the expected runtime of several common samplingbased planners. We propose beginning the search in a lower dimensional subspace of in the hopes that a simple solution will be found quickly. After a certain number of samples are generated, if no solution is found, we increase the dimension of the search subspace by 1 and continue sampling in the larger subspace. We repeat this process until a solution is found. In the worst case, the search subspace expands to include the full dimensional configuration space — making completeness properties identical to the underlying samplingbased planer. In many cases our experiments indicate that a solution is found much faster using this approach and the run time appears to be less sensitive to the full dimension of the configuration space.
An important property of these subspaces is that a solution entirely lying inside these subspaces should be feasible, in the absence of obstacles, so the initial and goal configurations must lie inside every subspace. The proposed method, by construction, generates subspaces that satisfy this constraint.
To evaluate this approach, we modified three well established planners, RRT [3], RRTConnect [16], and BidirectionalTRRT [17] to produce RRT, RRTConnect, and BidirectionalTRRT, with the symbol indicating that the planners are enhanced using the idea described above. All three planners were compared to the originals and to KPIECE [18]
and STRIDE
[10]. These planners were tested on a planar hyperredundant arm and on a simulated Baxter humanoid robot, shown in Figure 1, through OMPL [36] and the MoveIt! framework [19]. The results show that even a seemingly straightforward method for defining such subspaces can still have superior planning time performance for many problems compared to stateoftheart planners.The remainder of this paper is structured as follows. Section II reviews other approaches on planning for highdimensional systems. Section III provides technical details of the enhancement, considering important issues such as how the search subspaces is selected, how to generate the samples, and when to expand the search dimension. Section IV presents our experience enhancing three well established planners RRT [3], RRTConnect [16], and BidirectionalTRRT [17]. Finally, Section V concludes with a discussion of future work.
Ii Related work
The problem of motion planning has been proven to be PSPACEhard [1]. During the late 1990’s, samplingbased methods were introduced and shown to be capable of solving challenging motion planning problems, but without guarantees of finding the solution in finite time [20]. The two most prominent representatives of those algorithms are probabilistic roadmaps (PRMs) by Kavraki et al. [4] that are useful for multiple queries in a known, generally stable environment, and RRTs by LaValle [3], that are more suitable for single query applications. Two other variations of RRTs, RRTConnect by Kuffner and LaValle [16] that extends the tree more aggressively in each iteration, and TRRT by Jaillet et al. [17] that plans efficiently in costmaps, were used in this study.
Although the performance of these techniques can be affected substantially by the degrees of freedom, some studies have used them successfully for highdimensional configuration spaces. A method that uses PRMs and finds collisionfree paths for hyperredundant arms was presented by Park et al. [21]. Other studies use RRTs for motion planning of redundant manipulators, such as the work of Bertram et al. [22], which solves the inverse kinematics in a novel way. Weghe et al. [23] apply RRTs to redundant manipulators without the need to solve the inverse kinematics of the system. A study by Qian and Rahmani [24] combines RRTs and inverse kinematics in a hybrid algorithm in a way that drives the expansion of RRTs by the Jacobian pseudoinverse.
Additionally, some works use RRTs for mobile manipulators. Vannoy et al. [25] propose an efficient and flexible algorithm for operating in dynamic environments. The work of Berenson et al. [26] provides an application of their technique to a 10DoF mobile manipulator.
For multirobot systems, many samplingbased algorithms have been proposed. The study of van den Berg and Overmars [7] uses a PRM and presents a prioritized technique for motion planning of multiple robots. Other studies use RRTbased algorithms such as the study by Carpin and Pagello [27] which introduced the idea of having multiple parallel RRTs for multirobot systems. The work of Wagner [8] plans for every robot individually and, if needed, coordinates the motion in higher dimensional spaces. Other studies propose efficient solutions by using a single RRT [28, 29].
Furthermore, motion planning for humanoid robots with samplingbased algorithms has been explored; Kuffner et al. presented algorithms for motion planning on humanoid robots with both the use of PRMs [30] and RRTs [31]. Other studies, such as the work of Liu et al. [32], use RRTs for solving the stepping problem for humanoid robots.
The work of Vernaza and Lee tried to extract structural symmetries in order to reduce the dimensionality of the problem [9] by also providing nearoptimal solutions. This technique is more time efficient than the traditional RRT, mostly for very highdimensional configuration spaces. Yershova et al. [33] proposed an approach to focus sampling in the most relevant regions.
Finally, other approaches attempt to deal with the dimensionality in different ways. Gipson et al. developed STRIDE [10] which samples nonuniformly with a bias toward unexplored areas of the configuration space. KPIECE [18] by Şucan and Kavraki uses random 2D and 3D projections to estimate the coverage of . Additionally, Gochev et al. [11] proposed a motion planner that decreases the dimensionality by recreating a configuration space with locally adaptive dimensionality. Yoshida’s work [12] tries to sample in ways that exploit the redundancy of a humanoid system. Kim et al. [13] present an RRTbased algorithm for articulated robots that reduces the dimensionality of the problem by projecting each sample into subspaces that are defined by a metric. Shkolnik and Tedrake [34] plan for highly redundant manipulators in the dimensional task space with the use of Jacobian transpose and Voronoi bias. Wells and Plaku [14] reduce the dimensionality for 2D hyperredundant manipulators by modeling the endeffector as a single mobile robot, and the other links as trailers that are getting pulled. Lastly, the work of Bayazit et al. [35] where a PRM was used to plan in subspaces of the configuration space, creates paths that solve an easier problem than the original and then iteratively optimize the solution until the solution becomes valid.
This paper introduces the idea of iteratively searching in lowerdimensional subspaces, and emphasizes the potential of using such approach for efficient motion planning on arbitrary hyperredundant systems. The approach operates regardless of if there is an articulated robot, a humanoid, or a heterogeneous multirobot system, in a sense that additional limitations are not introduced in the original planners. Unlike previous works, the approach tries to find paths that are not only confined entirely to a subspace but also in a subspace in which a solution can exist, since the initial and goal configurations are part of the subspace. Contrary to the work of Bayazit et al. [35] the approach searches in subspaces that are strictly lowerdimensional. Intuitively, instead of finding results faster by simplifying the problem, our approach tries to find plans fast by simplifying the model of the system by applying virtual constraints, and then by iteratively relaxing them.
Iii Methodology
Iiia Problem Statement
Let denote a configuration space with degrees of freedom, partitioned into free space and obstacle space with . The obstacle space is not explicitly represented, but instead can be queried using collision checks on single configurations or short path segments. Given initial and goal configurations , we would like to find a continuous path within from to .
For purposes of sampling, we assume that each degree of freedom in is parameterized as an interval subset of , so that
(1) 
Note that we treat as Euclidean only in the context of sampling; other operations such as distance calculations and the generation of local path segments utilize identifications on the boundary of as appropriate for the topology.
IiiB Planning in Subspaces
The proposed method is based on searching for a solution in lower dimensional subspaces of , in hopes that such a path might be found faster than expanding the tree in all dimensions. The underlying idea is to exploit the redundancy of each system for each problem. To achieve this, the algorithm starts searching in the unique linear 1dimensional subspace of that contains and . If this search fails, the planner expands its search subspace by one dimension. This process continues iteratively until the planner finds a path, or until it searches in all of . In each subsearch the tree structure created in lower dimensions is kept and expanded in subsequent stages.
Algorithm 1 summarizes the general approach, as applied to RRT. The planner starts optimistically by searching in one dimension, along the line passing through and . If this search fails to find a path—a certainty, unless there are no obstacles between and —the search expands to a planar subspace that includes and , then to a 3D flat^{1}^{1}1We use the term flat to refer to a subset of congruent to some lowerdimensional Euclidean space., and so on until, in the worst case, the algorithm eventually searches all of ; see Figure 2.
The description in Algorithm 1 leaves three important elements unspecified. First, the algorithm needs a method for selecting and representing the subspace (Lines 1 and 1). Second, a method is required for sampling from this subspace (Line 1). Third, the conditions that must be met before moving to the next subsearch must be defined (Line 1). The choices explored in this study are described in the next sections.
IiiC Representing and Sampling from Subspaces
The central idea is to search for solutions in subspaces of progressively higher dimensions. The primary constraint on these subspaces is that they must contain both and . Subspaces that violate this constraint cannot, of course, contain a path connecting to .
In general, the algorithm’s selections for should ideally be directed by the likelihood that a solution will exist fully within .
The choice that we investigate in this paper—one that trades some generality for simplicity— is a prioritized release of the degrees of freedom. Given a set of DoFs to be constrained, we can form by constraining the Dofs in to form a line passing from and and allowing the remaining DoF to vary freely.
Next, the algorithm requires a technique for drawing samples from . The sampling happens in a very efficient linear time method that initially generates a sample within along the line between and by selecting a random scalar and applying:
(2) 
The algorithm then modifies by inserting, for each DoF not in , a different random value within the range for that dimension; see Algorithm 2.
To ensure that the original sample along the line between and is within , we compute and using Algorithm 3 and select randomly from the interval . The ComputeBoundaryValues function finds the intersections of this line all the different and flats, and defines the limits of from the values of producing the two intersected points that lie inside the . The ComputeBoundaryValues function is called only once before the planning loop.
The prioritized method provides an efficient and easy way to sample from projections of arbitrary dimensionality, while, as stated before, it provides valuable understanding that serves the purpose of our study.
IiiD Terminating the subsearches
The only remaining detail to be discussed is how long the search in each subspace should continue. A set of timeouts {} should be generated where corresponds to the amount of time spent in the iteration. Ideally, the planner should stop searching in subspaces that seem unlikely to provide a solution. For simplicity in this paper we precompute the timeouts by assuming that the follow a geometric progression. The idea is to exponentially increase the number of samples in each successive subsearch, acknowledging the need for more samples in higher dimensions.
The proposed approach uses two different parameters specified by the user. The first parameter is the timeout for the entire algorithm. The second parameter is a factor describing a constant ratio of the runtime between successive subsearches. The total time available to the algorithm can be expressed in terms of and a base time :
(3) 
Solving for we obtain:
(4) 
Using this every can be computed as:
(5) 
In this study values for ranging between and and scaling linearly with the dimensionality of the configuration space seem to lead to good performance.
Iv Experiments
For the experiments three new planners were developed using the OMPL framework [36]. These planners work by applying the technique to three RRT variants: (1) RRT [3] with default goal bias of , (2) RRTConnect [16], and (3) the bidirectional TRRT [17] by assuming a uniform costmap. The TRRT variant is intended to test the effect of our method to a powerful costmap planner.
Iva Experiments with a 2D hyperredundant manipulator
In order to test the ability of the new planners to adapt to different problems, the prioritization was chosen randomly for each run, although an optimization might be expected to give results faster. We demonstrate that given enough redundancy even a random prioritization will provide results much faster. A computer with 6th Generation Intel Core i76500U Processor (4MB Cache, up to 3.10 GHz) and 16GB of DDR3L (1600MHz) RAM was used.
The OMPL’s standard example of a 2D hyperredundant manipulator, created for STRIDE [10], was used in order to test and compare the enhanced planners in a standard way. Two different environments were tested 100 times each for a kinematic chain with varying 12 to 20 degrees of freedom: A Cluttered Random environment and a Horn environment; see Figure 3. The initial and goal configurations were the same as shown in Figure 3, so a qualitative comparison in problems with different redundancies could be done.
In all the cases, the enhanced versions of each planner were faster and in most cases significantly faster than the original ones. The average and the median times for the Random Cluttered environment are presented in Figure 4, and in Figure 5 for the Horn environment. Although the plots of the path length are not provided, our planners produced solutions with length — after the standard simplification method provided by OMPL — very similar, if not slightly better, than the other planners.
The BiTRRT not only outperformed the BiTRRT by a wide margin but also outperformed all the other planners using uniform sampling. Additionally, it provided competitive results to robust planners with biased sampling such as KPIECE [18] and STRIDE [10], which as expected perform much better in less redundant environments.
Interestingly, for each problem, the single fastest solution across all trials was generated by BiTRRT. This suggests that a good choice of prioritization may give results faster in a consistent way.
Additional experiments with the fastest planners were done from 12 to 30 dimensions for the Cluttered environment. The performance of the BiTRRT in Figure 6, it deals much better with the dimensionality. Also the BiTRRT provides the fastest results among all the planners for the most part. Even with random prioritization only after the 29 dimensions due to insufficient prioritizations the BiTRRT performs slower than KPIECE and STRIDE.
IvB Experiments with Baxter
Experiments are presented for the Baxter humanoid robot (Figure 1) of 14 degrees of freedom using the OMPL [36] and MoveIt! framework [19] with an Intel i77700 8core processor (3.6GHz), and 32 GiB RAM.
Instead of choosing the prioritizations randomly, a generic task independent policy was used to show that even naive policies can eliminate the outliers observed in the other experiments and lead to superior performance. The policy was giving priority to the joints closer to the base.
We tested the enhanced planners in a cluttered workspace, consisted by a table and four parallel pillars. As shown in Figure 7, Baxter starts with the manipulators in the relaxed position below the table and the goal is to reach the configuration that both arms fit between the pillars. Moreover, in order to make the problem more challenging for the enhanced planners, one of the manipulators should end up above the other, reducing even more the redundancy of the problem, forcing our planners to explore subspaces with high dimensionality. The RRT, RRTConnect, BiTRRT were compared with their original versions, with KPIECE and bidirectional version of KPIECE, provided in OMPL, called BiKPIECE for 100 trials. A timeout of 60 seconds was used for each run.
As shown in Figure 8, every single enhanced planner, performed much better that the corresponding ones. RRTConnect with BiTRRT much better than BiKPIECE, and KPIECE, whose results presented in Figure 9. Non bidirectional planners had difficulty finding a solution, showing the difficulty of the problem close to the goal region. The enhanced planners, even when they were failing in the case of RRT, produced solutions very quickly showing the advantage of sampling in lowerdimensional subspaces. Similarly to the experiments presented in the previous section, the fastest planner among all was the BiTRRT. Dominantly the solutions from RRT were found in a 8dimensional subspace, and from RRTConnect and BiTRRT in a 6dimensional.
V Conclusion
We studied a novel method for potentially accelerating motion planning in high dimensional configuration spaces by sampling in subspaces of progressively increasing dimension. The method provides, on average, solutions much faster than the original RRTbased methods.
The approach is general enough to be applied to a broad variety of motion planning problems. For example, our experiments show potential for planning with costmaps via adaptation of the bidirectional TRRT. The enhanced planners can also be adjusted to each problem and provide results even faster. The study clearly shows that such methods have great potential in solving very fast, seemingly difficult problems and further investigation is needed.
Future work will target a number of important questions. First, it would be very interesting to find an efficient way to choose subspaces that are more likely to contain solutions. Second, it is important to provide a general way to identify when a new iteration should begin, using a metric of the expansion of the tree, and eliminating the parameters. Prior work [5] provides an estimation of the coverage in an efficient analytical way. Moreover, by using the previous metric, it is possible to identify when the tree overcame a difficult area and then reduce the dimensionality of the search, in order to accelerate the results further. For both of the above problems discrete methods such as the one of Şucan and Kavraki [15] should be also considered.
Currently, there are two planners in OMPL that in some cases outperform the RRT planners in our experiments: KPIECE which uses random 2D or 3D projections to estimate the coverage efficiently and STRIDE which samples nonuniformly with a bias to narrow spaces. Our study can extend the first planner to sample strictly in subspaces of and use 2D, 3D or 4D projections to estimate the coverage, and the second planner, can be used with the enhancement to accelerate the solutions in the lower dimensional subspaces where narrow passages are expected to be common. The integration of the ideas underlying the proposed method with those planners to accelerate the results is left for future work.
Lastly, we plan to explore the ability of the method to efficiently produce paths that satisfy some natural constraints of each system. Since the subspaces are defined only by simple constraints between the DoFs, subspaces that satisfy some dynamic constraints can be explored by defining dynamic constraints between the different DoFs. Similarly, the ability of the method to find alternative solutions by avoiding exploring nondesired areas shows some potential.
Acknowledgment
The authors would like to thank the generous support of the Google Faculty Research Award and the National Science Foundation grants (NSF 0953503, 1513203, 1526862, 1637876).
References
 [1] J. H. Reif, “Complexity of the generalized mover’s problem.” Harvard University, Cambridge, MA Aiken Computation Lab, Tech. Rep., 1985.
 [2] J. Canny, The complexity of robot motion planning. MIT press, 1988.
 [3] S. M. LaValle, “Rapidlyexploring random trees: A new tool for path planning,” Computer Science Dept., Iowa State University, Tech. Rep. TR 9811, Oct. 1998.
 [4] L. E. Kavraki, P. Švestka, J.C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in highdimensional configuration spaces,” IEEE Trans. on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.
 [5] J. M. Esposito, “Conditional density growth (cdg) model: a simplified model of rrt coverage for kinematic systems,” Robotica, vol. 31, no. 05, pp. 733–746, 2013.
 [6] M. Xanthidis, K. J. Kyriakopoulos, and I. Rekleitis, “Dynamically Efficient Kinematics for HyperRedundant Manipulators,” in The 24th Mediterranean Conf. on Control and Automation, Athens, Greece, Jun. 2016, pp. 207–213.
 [7] J. P. Van Den Berg and M. H. Overmars, “Prioritized motion planning for multiple robots,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2005, pp. 430–435.
 [8] G. Wagner, “Subdimensional expansion: A framework for computationally tractable multirobot path planning,” Master thesis, 2015.
 [9] P. Vernaza and D. D. Lee, “Efficient dynamic programming for highdimensional, optimal motion planning by spectral learning of approximate value function symmetries,” in IEEE Int. Conf. on Robotics and Automation, 2011, pp. 6121–6127.
 [10] B. Gipson, M. Moll, and L. E. Kavraki, “Resolution independent density estimation for motion planning in highdimensional spaces,” in IEEE Int. Conf. on Robotics and Automation, 2013, pp. 2437–2443.
 [11] K. Gochev, B. Cohen, J. Butzke, A. Safonova, and M. Likhachev, “Path planning with adaptive dimensionality,” in Fourth annual symposium on combinatorial search, 2011.
 [12] E. Yoshida, “Humanoid motion planning using multilevel dof exploitation based on randomized method,” in 2005 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2005, pp. 3378–3383.
 [13] D.H. Kim, Y.S. Choi, T. Park, J. Y. Lee, and C.S. Han, “Efficient path planning for highDOF articulated robots with adaptive dimensionality,” in IEEE Int. Conf. on Robotics and Automation, 2015, pp. 2355–2360.
 [14] A. Wells and E. Plaku, “Adaptive samplingbased motion planning for mobile robots with differential constraints,” in Conference Towards Autonomous Robotic Systems. Springer, 2015, pp. 283–295.
 [15] I. A. Şucan and L. E. Kavraki, “On the performance of random linear projections for samplingbased motion planning,” in Intelligent Robots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference On. IEEE, 2009, pp. 2434–2439.
 [16] J. J. Kuffner and S. M. LaValle, “RRTconnect: An efficient approach to singlequery path planning,” in IEEE Int. Conf. on Robotics and Automation, vol. 2, 2000, pp. 995–1001.
 [17] L. Jaillet, J. Cortés, and T. Siméon, “Transitionbased RRT for path planning in continuous cost spaces,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2008, pp. 2145–2150.
 [18] I. A. Şucan and L. E. Kavraki, “Kinodynamic motion planning by interiorexterior cell exploration,” in Algorithmic Foundation of Robotics VIII. Springer, 2009, pp. 449–464.
 [19] S. Chitta, I. Sucan, and S. Cousins, “Moveit![ros topics],” IEEE Robotics & Automation Magazine, vol. 19, no. 1, pp. 18–19, 2012.
 [20] S. M. LaValle, Planning Algorithms. Cambridge Univ. press, 2006.
 [21] J. J. Park, H. S. Kim, and J.B. Song, “Collisionfree path planning for a redundant manipulator based on PRM and potential field methods,” Journal of Institute of Control, Robotics and Systems, vol. 17, no. 4, pp. 362–367, 2011.
 [22] D. Bertram, J. Kuffner, R. Dillmann, and T. Asfour, “An integrated approach to inverse kinematics and path planning for redundant manipulators,” in Proc. IEEE Int. Conf. on Robotics and Automation, 2006, pp. 1874–1879.
 [23] M. V. Weghe, D. Ferguson, and S. S. Srinivasa, “Randomized path planning for redundant manipulators without inverse kinematics,” in 7th IEEERAS Int. Conf. on Humanoid Robots, 2007, pp. 477–482.
 [24] Y. Qian and A. Rahmani, “Path Planning Approach for Redundant Manipulator Based on Jacobian PseudoinverseRRT Algorithm,” in 6th Int. Conf. on Intelligent Robotics and Applications, Busan, South Korea, Sep. 2013, pp. 706–717.
 [25] J. Vannoy and J. Xiao, “Realtime adaptive motion planning (ramp) of mobile manipulators in dynamic environments with unforeseen changes,” IEEE Trans. on Robotics, vol. 24, no. 5, pp. 1199–1212, 2008.
 [26] D. Berenson, J. Kuffner, and H. Choset, “An optimization approach to planning for mobile manipulation,” in IEEE Int. Conf. on Robotics and Automation, 2008, pp. 1187–1192.

[27]
S. Carpin and E. Pagello, “On parallel RRTs for multirobot systems,” in
Proc. 8th Conf. Italian Association for Artificial Intelligence
, 2002, pp. 834–841.  [28] T. Otani and M. Koshino, “Applying a path planner based on RRT to cooperative multirobot boxpushing,” Artificial Life and Robotics, vol. 13, no. 2, pp. 418–422, 2009.
 [29] K. Solovey, O. Salzman, and D. Halperin, “Finding a needle in an exponential haystack: Discrete rrt for exploration of implicit roadmaps in multirobot motion planning,” in Algorithmic Foundations of Robotics XI. Springer, 2015, pp. 591–607.
 [30] J. J. Kuffner Jr, S. Kagami, K. Nishiwaki, M. Inaba, and H. Inoue, “Dynamicallystable motion planning for humanoid robots,” Autonomous Robots, vol. 12, no. 1, pp. 105–118, 2002.
 [31] J. Kuffner, K. Nishiwaki, S. Kagami, M. Inaba, and H. Inoue, “Motion planning for humanoid robots,” in The Eleventh Int. Symposium Robotics Research, 2005, pp. 365–374.
 [32] H. Liu, Q. Sun, and T. Zhang, “Hierarchical RRT for humanoid robot footstep planning with multiple constraints in complex environments,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2012, pp. 3187–3194.
 [33] A. Yershova, L. Jaillet, T. Siméon, and S. M. LaValle, “Dynamicdomain RRTs: Efficient exploration by controlling the sampling domain,” in Proc. of the IEEE Int. Conf. on Robotics and Automation, 2005, pp. 3856–3861.
 [34] A. Shkolnik and R. Tedrake, “Path planning in 1000+ dimensions using a taskspace voronoi bias,” in IEEE International Conference on Robotics and Automation. IEEE, 2009, pp. 2061–2067.
 [35] O. B. Bayazit, D. Xie, and N. M. Amato, “Iterative relaxation of constraints: A framework for improving automated motion planning,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 2005, pp. 3433–3440.
 [36] I. A. Şucan, M. Moll, and L. E. Kavraki, “The open motion planning library,” IEEE Robotics & Automation Magazine, vol. 19, no. 4, pp. 72–82, 2012.