Bidirectional Heuristic Search for Motion Planning with an Extend Operator Allen Cheng, Dhruv Mauria Saxena, and Maxim Likhachev Abstract— Sampling-based approaches are often favored in inconsistent and exhibit high variance, whereas those found robotics for high-dimensional motion planning for their fast by the more deterministic search-based planners are fairly exploration of the search space. However, at best they offer consistent with each other.1 asymptotic guarantees on solution quality due to their inherent stochasticity. While planning, the majority of effort is often Nicholson first proposed bidirectional search as two sep- spent near the start and goal configurations with a large arate searches initialized from both the start and goal [2]. amount of free space in between. Bidirectional approaches In theory, these planners can exponentially reduce the num- such as RRT-Connect exploit this fact by greedily extending ber of expanded states, making them an attractive class and connecting search frontiers that simultaneously propagate of algorithms. It is well-known that informative heuristics from the start and goal configurations of a planning problem. In this work, we use such an extend operator for bidirectional significantly improve search efficiency for unidirectional heuristic search-based planners, which typically struggle with search, so using them for both searches in a bidirectional high-dimensionality. In doing so, we address the difficulty that algorithm is an intuitive extension. In practice, however, the these bidirectional planners face with connecting frontiers of frontiers of both search efforts often miss each other entirely, both search efforts while providing suboptimality bounds on without any overlap in the search space. The searches can solution quality. We validate our simple approach on high- dimensional manipulation tasks, demonstrating significantly end up expending significantly more effort in these cases, reduced search effort when compared against other popular contrary to the motivating intuition. This problem is exacer- bidirectional algorithms, both search-based and sampling. Our bated in high-dimensional state spaces, which are commonly algorithm maintains theoretical guarantees on suboptimality found in robotic motion planning problems. Bidirectional and completeness for a given resolution. In addition, the sampling-based approaches on the other hand, such as the solutions found by our planner are of higher quality compared to those found by the other baseline algorithms. RRT-Connect algorithm [3], offer fast planning performance and scale well with high-dimensionality but at the cost of I. INTRODUCTION deterministic theoretical guarantees on solution quality and completeness. Consider the manipulation problem of pick-and-place in In our work, we present a simple but effective extension cluttered environments with a high degree-of-freedom (DoF) that aims to overcome the known shortcomings of bidirec- robotic arm, a challenge commonly faced in industrial set- tional heuristic search with the use of an extend operator. tings. Successful grasping often requires tight and minimal The extend operator is used to connect the search frontiers tolerances due to clutter around the start and goal configura- being explored simultaneously but independently from the tions. This demands a large amount of search effort around start and goal configurations. This operator was originally the start and goal region, but the majority of the intermediate proposed as a greedy heuristic for connecting forward and path is typically collision-free. Naive search-based planners backward searches in bidirectional sampling-based planners require procedural reasoning over the possible arm configu- [3]. Motivated by the example described earlier, we adopt rations when planning through this free space to maintain op- this operator for the heuristic search setting to efficiently timality guarantees. To increase planning efficiency in these reason through free space in high DoF motion planning. With scenarios, biased heuristics may be used, but they suffer our modification, we maintain consistency and deterministic when presented with cluttered environments that induce local bounds on the suboptimality of found solutions, for a given minima. Planning with adaptive dimensionality [1] exploits resolution of action and state space. this insight to quickly find paths in low-dimensional repre- sentations before executing a high-dimensional search in a II. RELATED WORK tunnel around those paths. Despite these advances, sampling- A. Bidirectional Planning Algorithms based approaches are often still favored for planning in high-dimensions due to their attractive planning speeds. 1) RRT-Connect: The RRT-Connect algorithm [3] first By abandoning deterministic guarantees on optimality of introduced the extend function in the context of motion found solutions, sampling-based planners randomly probe planning without kinodynamic constraints. The sampling- the configuration space to quickly find feasible trajectories. based planner dynamically constructs two rapidly-exploring As a result, the solutions found by these planners can be random trees [4] (from the start and goal configurations). It attempts to connect both trees using an EXTEND operator. All authors are affiliated with The Robotics Institute, Carnegie Mellon University, Pittsburgh, USA fallenc1, dsaxena, 1Consistency refers to the likeness between paths with similar start-goal [email protected] pairs with respect to some metric. This operator takes a single, fixed-size step towards the heuristic to guide front-to-front search, an extend operator nearest-neighbor in the opposite tree. A CONNECT function is used to greedily extend to the opposite tree. repeatedly executes EXTEND until a node in the opposite B. Tree Extension tree is reached or an obstacle blocks any further extension attempts. The greedy nature of RRT-Connect leads to fast 1) EXTEND/ CONNECT Operators: The extend operator performance and its sampling behavior allows it to trans- responsible for connecting pairs of states has been widely late well to high-dimensional motion planning problems. explored in robotic motion planning. The extend operator’s However, the algorithm has no guarantees on optimality job is two-fold: it must select a state as a local goal or bounded suboptimality, and solution quality can vary using a distance metric and perform the extension itself. greatly. Recent work [5], [6] has attempted to address this For bidirectional sampling-based methods, nearest-neighbors by adopting the local rewiring procedure from RRT* [7], but (NN) are selected as extension targets. For problems with these approaches can only guarantee asymptotic optimality. kinematic or kinodynamic constraints, the extension must 2) Heuristic Search-Based Algorithms: Pohl first ex- solve a two-point boundary value problem (BVP). Lavalle plored the combination of bidirectional and heuristic search [15] proposes motion primitives that describe a predefined with the classical front-to-back bidirectional heuristic search set of discretized control actions, but this does not fully solve algorithm, BHPA [8]. However, the theoretical benefits were the BVP. Prior research has offered solutions that include the never consistently reflected in experiments. It was later noted use of optimal controllers [16] and splines [17]. Since we use that the frontiers for forward and backward search often the same nearest-neighbors extend operator in our approach, missed or crossed through each other in practice [9]. This these methodologies and adaptations to various domains can observation, considered to be crux of bidirectional heuristic be easily adopted into our algorithm. search, motivated Champeaux and Sint to develop the Bidi- 2) Adaptive Motion Primitives: Cohen, et al. use adaptive rectional Heuristic Front-to-Front Algorithm (BHFFA) [10]. motion primitives [18] that are generated on-the-fly during BHFFA achieves reduced expansions compared to BHPA at search. In the example scenario we introduced previously, the cost of having to exhaustively compute a front-to-front they are used to connect the search frontier to the goal con- distance for entire frontiers. For high-dimensional motion figuration. The goal may be underdefined vis-a-vis´ degrees- planning, this is prohibitively expensive to compute. Recent of-freedom of the robot and thus differ from the dimensional- research has focused on ensuring the two search efforts ity of the search space. For example, we may specify a 6 DoF “meet in the middle” through the use of a unique priority goal pose in SE(3) while planning for a 7 DoF robot arm function [11]. However, this property has no clear use in (i.e. planning for 7 joints). The redundant joint allows for the context of complex motion planning, where frontier a set of states that satisfy the goal constraint. An analytical intersection may not always be in the middle. inverse kinematics (IK) solver is used to generate a valid 3) A*-Connect: The recently introduced A*-Connect al- state when the search is sufficiently close to the goal. A gorithm [12] leverages a Multi-Heuristic A* (MHA*) [13] dynamically constructed motion primitive is used to reach framework to guide search bidirectionally towards both the the goal state. For the robotic arm, this translates into to opposite root and frontier in parallel. MHA* provides a a linearly-interpolated path between the valid search state structured framework enabling the use of multiple inadmis- and the goal