Consider the problem of planning a custom robot's behavior for the task of autonomously unloading boxes from trucks. The system is required to reason what is the most beneficial sequence of feasible actions that will allow to safely complete the given task as fast as possible. This is a complex task due to imperfect information (what are the masses of the boxes in the truck?), non-trivial cost functions (is it better to attempt aggressive actions that may jam the system but unload multiple boxes or settle for a more conservative approach?), a huge spectrum of possible settings (was the truck neatly packed or were the boxes tossed into the truck?) and more. The complexity of the task can be partially ameliorated by using simulators in an offline stage to examine different scenarios and precompute approaches or strategies for recurring settings. This raises a multitude of questions such as how to efficiently construct such strategies? How should they be used online? and how to efficiently use the simulator. In [1] we present a robotic planning, learning and reasoning system for a truck unloading application. In [2] we present our approach for speeding up simulations-an essential building for our system.

[1] Oren Salzman, Andrew Dornbush, Fahad Islam, Sung-Kyun Kim, Anirudh Vemula, and Maxim Likhachev "Planning, learning and reasoning for robot truck unloading---a system's case study".

[2] Abhijeet Tallavajhula, Adrian Schoisengeier, Sung-Kyun Kim, Anirudh Vemula, Levi Lister and Oren Salzman "Task-Informed Fidelity Management for Speeding Up Robotics Simulation".

Inspection planning, the task of planning motions that allow a robot to inspect a set of points of interest, has applications in domains such as industrial, field, and medical robotics. Inspection planning can be computationally challenging, as the search space over motion plans grows exponentially with the number of points of interest to inspect. We propose a novel method, Incremental Random Inspection-roadmap Search (IRIS), that computes inspection plans whose length and set of successfully inspected points asymptotically converge to those of an optimal inspection plan. IRIS incrementally densifies a motion planning roadmap using sampling-based algorithms, and performs efficient near-optimal graph search over the resulting roadmap as it is generated. We demonstrate IRIS’s efficacy on a simulated planar 5DOF manipulator inspection task and on a medical endoscopic inspection task for a continuum parallel surgical robot in cluttered anatomy segmented from patient CT data. We show that IRIS computes higher-quality inspection plans orders of magnitudes faster than a prior state-of-the-art method.

[1] Mengyu Fu, Alan Kuntz, Oren Salzman, Ron Alterovitz "Towards Asymptotically-Optimal Inspection Planning via Efficient Near-optimal Graph Search".

We present an algorithm that generates a collision-free configuration-space path that closely follows, according to the discrete Fréchet metric, a desired path in task space. By leveraging the Fréchet metric and other tools from computational geometry, we approximate the search space using a cross-product graph. This structure allows us to efficiently search for the solution using a simple variant of Dijkstra’s graph search algorithm. Additionally, we can incrementally update and improve the solution in an anytime fashion. We compare multiple proposed densification strategies and show that our algorithm outperforms a previously proposed optimization-based approach.

The algorithmic tools we developed can be applied to plan paths for concentric-tube robots to allow for minimally-invasive surgery. In this application, we envision the surgeon providing the reference path and my algorithm producing the actual path for either the tip of the tube robot or the bevel edge of the steerable needle that follows the reference path. We are currently studying this exciting application.

[1] Rachel Holladay, Oren Salzman, Siddhartha Srinivasa: "Minimizing Task Space Frechet Error via Efficient Incremental Graph Search".

[2] Sherdil Niyaz, Alan Kuntz, Oren Salzman, Ron Alterovitz, Siddhartha Srinivasa "Following Surgical Trajectories with Concentric Tube Robots via Nearest-Neighbor Graphs".

[3] Sherdil Niyaz, Alan Kuntz, Oren Salzman, Ron Alterovitz, Siddhartha Srinivasa "Optimizing Motion-Planning Problem Setup via Bounded Evaluation with Application to Following Surgical Trajectories".

Asymptotically-optimal motion planners such as RRT* have been shown to incrementally approximate the shortest path between start and goal states. Once an initial solution is found, their performance can be dramatically improved by restricting subsequent samples to regions of the state space that can potentially improve the current solution. When the motion planning problem lies in a Euclidean space, this region $C_inf$, called the informed set, can be sampled directly. However, when planning with differential constraints in non-Euclidean state spaces, no analytic solutions exists to sampling $C_inf$ directly.

State-of-the-art approaches to sampling $C_inf$ in such domains such as Hierarchical Rejection Sampling (HRS) may still be slow in high-dimensional state space. This may cause the planning algorithm to spend most of its time trying to produces samples in $C_inf$ rather than explore it. In this work [1], we suggest an alternative approach to produce samples in the informed set $C_inf$ for a wide range of settings. Our main insight is to recast this problem as one of sampling uniformly within the sub-level-set of an implicit non-convex function. This recasting enables us to apply Monte Carlo sampling methods, used very effectively in the Machine Learning and Optimization communities, to solve our problem. We show for a wide range of scenarios that using our sampler can accelerate the convergence rate to high-quality solutions in high-dimensional problems.

[1] Daqing Yi, Rohan Thakker, Cole Gulino, Oren Salzman, Siddhartha Srinivasa: "Generalizing Informed Sampling for Asymptotically Optimal Sampling-based Kinodynamic Planning via Markov Chain Monte Carlo".

Planning the motion for humanoid robots is a computationally-complex task due to the high dimensionality of the system. Thus, a common approach is to first plan in the low-dimensional space induced by the robot’s feet - a task referred to as footstep planning. This low-dimensional plan is then used to guide the full motion of the robot. One approach that has proven successful in footstep planning is using search-based planners such as A* and its many variants. To do so, these search-based planners have to be endowed with effective heuristics to efficiently guide them through the search space. However, designing effective heuristics is a time-consuming task that requires the user to have exceptional domain knowledge. Thus, our goal is to be able to effectively plan motions taken by a humanoid robot while obviating the burden on the user to carefully design local-minima free heuristics.

For footstep planning, we propose to use user-defined homotopy classes in the workspace that are intuitive to define [1]. These homotopy classes are used to automatically generate heuristic functions that efficiently guide the search algorithm. On the analytical side, we provide guarantees on completeness within a specified suboptimality bound. Experimentally, we apply our approach to a 6DOF pair of humanoid feet and show the benefits over the existing methods.

For following the footstep plan in the high-dimensional space of the robot, we suggest a planning framework where a motion-planning algorithm can obtain guidance from a user [2]. In contrast to existing approaches, we suggest to seek user guidance only when the planner identifies that it ceases to make significant progress towards the goal. User guidance is given in the form of an intermediate configuration q which, in turn, is used to bias the planner to go through q. We demonstrate our approach for the case where the planning algorithm is Multi-Heuristic A* (MHA*) and the robot is a 34-DOF humanoid. We show that using this general approach allows to compute highly-constrained paths such as climbing stairs with little domain knowledge.

[1] Vinitha Ranganeni , Oren Salzman, Maxim Likhachev: "Footstep Planning for Humanoids Using Homotopy-Class Guidance".

[2] Fahad Islam, Oren Salzman, Maxim Likhachev: "Online, interactive user guidance for high-dimensional, constrained motion planning".

Motion-planning problems, such as manipulation in cluttered environments, often require that a collision-free shortest path be computed quickly given a roadmap graph G. Typically, the computational cost of evaluating if an edge of G is collision-free dominates the running time of the search algorithm. Indeed, algorithms such as Lazy Weighted A* (LWA*) and LazySP have been proposed to reduce the number of edge evaluations by employing a lazy lookahead (one-step lookahead and infinite-step lookahead, respectively). However, this comes at the expense of additional graph operations (the larger the lookahead, more the graph operations that are required). Since our ultimate goal is to minimize the \emph{total planning time}, we argue that a search algorithm should allow to balance expensive edge evaluations and graph operations. To this end, we present Lazy Receding-Horizon A* (LRA*) [1,3], a family of lazy shortest-path graph search algorithms. Each algorithm is endowed employs a lazy lookahead that generalizes LWA* and LazySP, and allow to balance edge evaluations and graph operations. We analyze the theoretic properties of LRA* and demonstrate empirically that, in many cases, to minimize the total planning time, the algorithm requires an intermediate lazy lookahead. Namely, using this intermediate lazy lookahead our algorithm outperforms both LWA* and LazySP.

If we are only interested in minimizing edge evaluations, then we also answer the question "how close to optimal is LazySP in terms of minimizing edge evaluations?" Our main result is an analytical upper bound, in a probabilistic model, on the number of edge evaluations required by LazySP algorithms; a matching lower bound shows that these algorithms are asymptotically optimal in the worst case [2].

[1] Aditya Vamsikrishna, Oren Salzman, Siddhartha S. Srinivasa: "Lazy Receding Horizon A* for Efficient Path Planning in Graphs with Expensive-to-Evaluate Edges".

[2] Nika Haghtalab, Simon Mackenzie, Ariel D. Procaccia, Oren Salzman, Siddhartha S. Srinivasa: "The Provable Virtue of Laziness in Motion Planning".

[2] Aditya Mandalika, Sanjiban Choudhury, Oren Salzman, Siddhartha S. Srinivasa: "Generalized Lazy Search for Robot Motion Planning: Interleaving Search and Edge Evaluation via Event-based Toggles".

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)