We consider the motion-planning problem of planning a collision-free path of a robot in the presence of risk zones. The robot is allowed to travel in these zones but is penalized in a super-linear fashion for consecutive accumulative time spent there. We suggest a natural cost function that balances path length and risk-exposure time. Specifically, we consider the discrete setting where we are given a graph, or a roadmap, and we wish to compute the minimal-cost path under this cost function. Interestingly, paths defined using our cost function do not have an optimal substructure. Namely, subpaths of an optimal path are not necessarily optimal. Thus, the Bellman condition is not satisfied and standard graph-search algorithms such as Dijkstra cannot be used. We present a path-finding algorithm, which can be seen as a natural generalization of Dijkstra’s algorithm. Our algorithm runs in $O((n_B n) log(n_B n) + n_B m)$ time, where $n$ and $m$ are the number of vertices and edges of the graph, respectively, and $n_B$ is the number of intersections between edges and the boundary of the risk zone. We present simulations on robotic platforms demonstrating both the natural paths produced by our cost function and the computational efficiency of our algorithm.
As an example, consider the scenario to the left, motivated by assistive robotics. A robot arm is performing a task such as pouring juice from a bottle, while receiving inputs from a user such as when to stop pouring. During the motion, the robot’s end effector is moving between regions that are either visible (colored green) or occluded (colored red) to the user, respectively. In this motion-planning problem configurations in the risk regions are points occluded from the viewpoint of a user sitting in a wheelchair. Snapshots are taken at intermediate points along the path. (a) Shortest path. (b) Minimal-cost path.
A technical report describing this work can be found here.
Interestingly, in the continuous setting it is not clear if this problem is computationally hard or not, even for the planar case. This open problem was presented in 12th International Workshop on the Algorithmic Foundations of Robotics (WAFR). Here is a video of the presentation.
We study a path-planning problem amid a set $O$ of obstacles in $R^2$, in which we wish to compute a short path between two points while also maintaining a high clearance from $O$; the clearance of a point is its distance from a nearest obstacle in $O$. Specifically, the problem asks for a path minimizing the reciprocal of the clearance integrated over the length of the path. We present the first polynomial-time approximation scheme for this problem. Let $n$ be the total number of obstacle vertices and let $\eps \in (0, 1]$. Our algorithm computes in time $O((n/\eps)^2 \log (n/\eps)) a path of total cost at most (1 + \eps) times the cost of the optimal path.
This work appeared in the proceedings of ACM-SIAM Symposium on Discrete Algorithms(SODA) 2016 and presented by Kyle Fox at the conference.
Roadmaps constructed by many sampling-based motion planners coincide, in the absence of obstacles, with standard models of random geometric graphs (RGGs). Those models have been studied for several decades and by now a rich body of literature exists analyzing various properties and types of RGGs. In their seminal work on optimal motion planning Karaman and Frazzoli [1] conjectured that a sampling-based planner has a certain property if the underlying RGG has this property as well. In this work we settle this conjecture and leverage it for the development of a general framework for the analysis of sampling-based planners. Our framework, which we call localization-tessellation, allows for easy transfer of arguments on RGGs from the free unit-hypercube to spaces punctured by obstacles, which are geometrically and topologically much more complex. We demonstrate its power by providing alternative and (arguably) simple proofs for probabilistic completeness and asymptotic (near-)optimality of probabilistic roadmaps (PRMs). Furthermore, we introduce several variants of PRMs, analyze them using our framework, and discuss the implications of the analysis.
Our localization-tessellation framework consists of two main components. First we show through localization that RGGs maintain their properties in arbitrarily-small neighborhoods. The tessellation stage extends these properties to complex domains which can be viewed as free spaces of motion-planning problems. Namely, the configuration space punctured by obstacles.
This work appeared in the proceedings of Robotics: Science and Systems (RSS) 2016 and was invited to a special issue of IJRR. Kiril Solovey also presented this work in the Workshop on Random Geometric Graphs, Banff International Research Center, AB, Canada. For a detailed technical report, see our extended arXiv version.
[1] Sertac Karaman, Emilio Frazzoli: Sampling-based algorithms for optimal motion planning. I. J. Robotics Res. 30(7): 846-894 (2011)
The complexity of nearest-neighbor search dominates the asymptotic running time of many sampling-based motion-planning algorithms. However, collision detection is often considered to be the computational bottleneck in practice. Examining various asymptotically optimal planning algorithms, in [1] we characterize settings, which we call NN-sensitive, in which the practical computational role of nearest-neighbor search is far from being negligible, i.e., the portion of running time taken up by nearest-neighbor search is comparable, or sometimes even greater than the portion of time taken up by collision detection. This reinforces and substantiates the claim that motion-planning algorithms could significantly benefit from efficient and possibly specifically-tailored nearest-neighbor data structures. The asymptotic (near) optimality of these algorithms relies on a prescribed connection radius, defining a ball around a configuration q, such that q needs to be connected to all other configurations in that ball. To facilitate our study, we show how to adapt this radius to non-Euclidean spaces, which are prevalent in motion planning. This technical result is of independent interest, as it enables to compare the radial-connection approach with the common alternative, namely, connecting each configuration to its k nearest neighbors (k-NN). Indeed, as we demonstrate, there are scenarios where using the radial connection scheme, a solution path of a specific cost is produced ten-fold (and more) faster than with k-NN.
In [2] we employ Randomly transformed grids (RTG) [3] for sampling-based motion-planning algorithms and describe an efficient implementation of the approach. We show that for motion-planning, RTG allow for faster convergence to high-quality solutions when compared with existing NN data structures. Additionally, RTG enable significantly shorter construction times for batched-PRM variants; specifically, we demonstrate a speedup by a factor of two to three for some scenarios.
[1] Michal Kleinbort, Oren Salzman, Dan Halperin: "Collision detection or nearest-neighbor search? On the computational bottleneck in sampling-based motion planning". Workshop on the Algorithmic Foundations of Robotics (WAFR) 2016.
[2] Michal Kleinbort, Oren Salzman, Dan Halperin: "Efficient high-quality motion planning by fast all-pairs r-nearest-neighbors". IEEE International Conference on Robotics and Automation (ICRA) 2015: 2985-2990.
[3] Dror Aiger, Haim Kaplan, Micha Sharir: "Reporting Neighbors in High-Dimensional Euclidean Space". SIAM J. Comput. 43(4): 1363-1395 (2014)
Our C++ RTG Implementation here. The reference manual can be found here.
We present a sampling-based framework which incorporates an implicit representation of a roadmap with a novel approach for pathfinding in geometrically embedded graphs. Our pathfinding algorithm, discrete-RRT (dRRT), is an adaption of the celebrated RRT algorithm, for the discrete case of a graph. We apply our framework for the case of multi-robot motion planning [1] and motion planning for multi-link robots [2].
By rapidly exploring the high-dimensional configuration space represented by the implicit roadmap, dRRT is able to reach subproblems where minimal coordination between the robots is required. Integrating the implicit representation of the roadmap, the dRRT algorithm, and techniques that are tailored for such subproblems on the implicit roadmap allows us to solve multi-robot problems while exploring only a small portion of the configuration space.
Our work on multi-link robot is based on the simple observation that the set of configurations for which the robot is self-collision free is independent of the obstacles or of the exact placement of the robot. This allows us to eliminate the need to perform costly self-collision checks online when solving motion-planning problems, assuming some offline preprocessing. In particular, given a specific robot our algorithm precomputes a tiling roadmap, which efficiently and implicitly encodes the self-collision free (sub-)space over the entire configuration space, where the latter can be infinite for that matter. To answer any query, in any given scenario, we traverse the tiling roadmap while only testing for collisions with obstacles. Our algorithm suggests more flexibility than the prevailing paradigm in which a precomputed roadmap depends both on the robot and on the scenario at hand. We show through various simulations the effectiveness of this approach. Specifically, in certain settings our algorithm is ten times faster than the RRT algorithm. .
[1] Kiril Solovey, Oren Salzman, Dan Halperin: "Finding a needle in an exponential haystack: Discrete RRT for exploration of implicit roadmaps in multi-robot motion planning". The International Journal of Robotics Research (IJRR) 35(5): 501-513 (2016). Also appeared in WAFR14 as well as in our extended arXiv report.
[2] Oren Salzman, Kiril Solovey, Dan Halperin: "Motion Planning for Multi-Link Robots by Implicit Configuration-Space Tiling". In IEEE Robotics and Automation Letters (RA-L), 1(2):760-767 (2016). See also our extended arXiv report.
We present Lower Bound Tree-RRT (LBT-RRT), a single-query sampling-based algorithm that is asymptotically near-optimal. Namely, the solution extracted from LBT-RRT converges to a solution that is within an approximation factor of $1+\eps$ of the optimal solution. Our algorithm allows for a continuous interpolation between the fast RRT algorithm and the asymptotically optimal RRT* and RRG algorithms when the cost function is path length. When the approximation factor is $1$ (i.e., no approximation is allowed), LBT-RRT behaves like RRG. When the approximation factor is unbounded, LBT-RRT behaves like RRT. In between, LBT-RRT is shown to produce paths that have higher quality than RRT would produce and run faster than RRT* would run. This is done by maintaining a tree which is a sub-graph of the RRG roadmap and a second, auxiliary graph, which we call the lower-bound graph. The combination of the two roadmaps, which is faster to maintain than the roadmap maintained by RRT*, efficiently guarantees asymptotic near-optimality. We suggest to use LBT-RRT` for high-quality, anytime motion planning. We demonstrate the performance of the algorithm for scenarios ranging from 3 to 12 degrees of freedom and show that even for small approximation factors, the algorithm produces high-quality solutions (comparable to RRG and RRT*) with little running-time overhead when compared to RRT.
The original version of this work appeared in ICRA 14. The journal version of this work appeared in TRO 16. The algorithm's implementation. is publicly distributed with the Open Motion Planning Library (OMPL).We present a general and modular algorithmic framework for path planning of robots. Our framework combines geometric methods for exact and complete analysis of low-dimensional configuration spaces, together with practical, considerably simpler sampling-based approaches that are appropriate for higher dimensions. In order to facilitate the transfer of advanced geometric algorithms into practical use, we suggest taking samples that are entire low-dimensional manifolds of the configuration space that capture the connectivity of the configuration space much better than isolated point samples. Geometric algorithms for analysis of low-dimensional manifolds then provide powerful primitive operations. The modular design of the framework enables independent optimization of each modular component.
We have developed, implemented and optimized a primitive operation for complete and exact combinatorial analysis of a certain set of manifolds, using arrangements of curves of rational functions and concepts of generic programming [1]. This in turn enabled us to implement our framework for the concrete case of a polygonal robot translating and rotating amidst polygonal obstacles [2]. We demonstrate that the integration of several carefully engineered components leads to significant speedup over the popular PRM sampling-based algorithm, which represents the more simplistic approach that is prevalent in practice.
We recursively apply the MMS framework in a six-dimensional configuration space enabling the coordination of two polygonal robots translating and rotating amidst polygonal obstacles. In the adduced experiments for the more demanding test cases MMS outperforms PRM with over 20-fold speedup in a coordination-tight setting [3].
We present a probabilistic completeness proof for the most prevalent case, namely MMS with samples that are affine subspaces. A close examination of the test cases reveals that MMS has, in comparison to standard sampling-based algorithms, a significant advantage in scenarios containing high-dimensional narrow passages. This provoked a novel characterization of narrow passages which attempts to capture their dimensionality, an attribute that had been (to a large extent) unattended in previous definitions.
[1] Ron Wein, Eric Berberich, Efi Fogel, Dan Halperin, Michael Hemmer, Oren Salzman, and Baruch Zukerman "2D Arrangements, CGAL - Computational Geometry Algorithms Library", release 4.0. CGAL, May 2012
[2] Oren Salzman, Michael Hemmer, Barak Raveh, Dan Halperin: "Motion Planning via Manifold Samples". Algorithmica 67(4): 547-565 (2013)
[3] Oren Salzman, Michael Hemmer, Dan Halperin: "On the Power of Manifold Samples in Exploring Configuration Spaces and the Dimensionality of Narrow Passages". IEEE Trans. Automation Science and Engineering 12(2): 529-538 (2015)
I have been fortunate to work with amazing people throughout my academic career. Here is a sample of the researches that I had the privilege to collaborated with.
I am co-organizing a workshop in RSS 2017 titled Mathematical Models, Algorithms, and Human-Robot Interaction. The workshop looks at two complementary disciplines required for robots to leave the cages of the industrial manufacturing production lines and the safety of research labs into the unstructured environments of everyday life. Specifically, such settings require notions and tools from algorithmic robotics, such as motion planning and machine learning, and human-robot interaction (HRI), such as cognitive modeling, intention recognition, and activity prediction. Both areas have much to contribute to each other in terms of methods, approaches, and insights, and yet the algorithmic robotics and human-robot interaction communities remain largely disjoint groups. There are four goals for this workshop: to (1) expose approaches from HRI that inform algorithmic models of humans, (2) identify mathematical and algorithmic tools to address HRI problems, (3) encourage conversation between researchers in the areas of HRI and fundamental algorithms for robotics, and (4) raise a series of open questions that fall in the intersection of algorithmic robotics and HRI.