{'fraction_non_alphanumeric': 0.04141327623126338, 'fraction_numerical': 0.019379014989293362, 'mean_word_length': 4.35500515995872, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 7, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Section Patterns: Efficiently Solving Narrow Passage Problems using Multilevel Motion Planning

Sampling-based planning methods often become inefficient due to narrow passages. Narrow passages induce a higher runtime, because the chance to sample them becomes vanishingly small. In recent work, we showed that narrow passages can be approached by relaxing the problem using admissible lower-dimensional projections of the state space. Those relaxations often increase the volume of narrow passages under projection. Solving the relaxed problem is often efficient and produces an admissible heuristic we can exploit. However, given a base path, i.e. a solution to a relaxed problem, there are currently no tailored methods to efficiently exploit the base path. To efficiently exploit the base path and thereby its admissible heuristic, we develop section patterns, which are solution strategies to efficiently exploit base paths in particular around narrow passages. To coordinate section patterns, we develop the pattern dance algorithm, which efficiently coordinates section patterns to reactively traverse narrow passages. We combine the pattern dance algorithm with previously developed multilevel planning algorithms and benchmark them on challenging planning problems like the Bugtrap, the double L-shape, an egress problem and on four pregrasp scenarios for a 37 degrees of freedom shadow hand mounted on a KUKA LWR robot. Our results confirm that section patterns are useful to efficiently solve high-dimensional narrow passage motion planning problems.


I. INTRODUCTION
Sampling-based motion planning algorithms are a successful paradigm to automate robotic tasks [51]. However, sampling-based algorithms do not perform well when the state space of the robot contains narrow passages [58,92,38,78], which are low-measure regions which have to be traversed to reach a goal. Narrow passages are often occurring in tasks which are particularly important in robotic applications, like grasping, peg-in-hole, egress/ingress or long-horizon planning problems [24,35].
In previous work, we and other research teams have shown that we can often efficiently solve high-dimensional planning problems by using admissible lower-dimensional projections of the state space, a topic we refer to as multilevel motion planning [22,5,67,75,97]. When using a multilevel motion planning framework, we can often use solutions to simplified planning stages as admissible heuristics for the original problem [70,1]. To efficiently exploit those admissible heuristics, we can use biased sampling methods [67,74] which we can combine with classical planning algorithms like the rapidly-exploring random tree algorithm [63], the probabilistic roadmap planner [65], its optimal star versions [67] or the fast marching trees planner [74]. However, while showing promising runtimes, those algorithms are prone to get trapped when run on problems involving narrow passages.
In this work, we address narrow passages in multilevel motion planning problems by developing section patterns. Section patterns are methods to explicitly address problematic situations which occur when we like to exploit solutions to relaxed problems.
We introduce four section patterns. First, we introduce the Manhattan pattern, which we use to compute solution paths which actuate the minimal amount of joints to reach a goal region, which is advantageous for high dimensional systems [14,67]. Second, we introduce the Wriggle pattern, which we use to make small random walk steps to traverse a narrow passage. Third, we introduce the Tunnel pattern, which we use to steer around small infeasible regions. Fourth, we introduce the Triple step pattern, which we use to backtrack in case the algorithm gets stuck. In Fig. 1 the Triple step pattern is showcased for a 37-degrees of freedom (dof) robotic hand. We execute the pattern when a collision occurs (1). We first backstep (2), then sidestep (3) and finally we make a forward step (4) to reach a goal position. The details of this and of the other pattern will be detailed later in this paper.
To coordinate the execution of the four section patterns, we develop a novel algorithm we call pattern dance. The pattern dance algorithm starts with an Manhattan pattern until a collision occurs. The algorithm then switches to an iteration of the wriggling pattern followed by the tunneling pattern. If those patterns fail, we switch to the triple step pattern. Once a pattern is successful, we recursively call the pattern dance algorithm until we either reach a certain depth or we reach the goal region. We embed the pattern dance algorithm into four multilevel planner [67], namely the quotient space RRT (QRRT) [63], the quotient space roadmap planner (QMP) [65] and its optimal versions QRRT* and QMP* [67].
Our contributions are as follows. 1) We develop section patterns to efficiently exploit base space paths (solutions to relaxed problems). 2) To coordinate sections patterns, we develop the pattern dance algorithm 3) We combine the pattern dance algorithm with four multilevel planner (QRRT, QRRT*, QMP, QMP*) and comparison against 36 planners from the open motion planning library (OMPL) and a previous sidestepping algorithm [67] on 7 challenging scenarios.

II. RELATED WORK
Let us review the literature by focusing on two topics. First, we focus on generating admissible heuristics [20] for motion planning problems involving continuous domains [51]. We discuss sources of admissible heuristics like constraint relaxations, lazy search, informed trees and past experience. Second, given an admissible heuristic, we review methods to efficiently exploit the heuristic either using path section approaches, local minima avoidance or narrow passage handling.

A. Generating Admissible Heuristics
Motion planning [51] is a well studied topic which has been successfully applied to a wide range of problem domains [62]. One of the most promising paradigms to solve motion planning problems are (optimal) sampling-based planner [45,82,81,6,25]. However, those planner might become inefficient in state spaces which are too high-dimensional [67], contain intricate constraints [42] or narrow passages [53]. We can, however, often solve such problems efficiently, if we use admissible heuristics [1].
We believe there are three large sources of admissible heuristics. First, we can compute admissible heuristics as solutions to relaxed problems [70]. Early instances of this idea to motion planning can be found in the constraint relaxation frameworks by Ferbach and Barraquand [22], Sekhavat et al. [85] and Bayazit et al. [5]. Newer instances of this idea are putting the focus on different aspects like the specific type of projection [90,30] or the type of lower-dimensional space [65,9]. We refer to all those frameworks under the collective term multilevel motion planning [67]. We can apply multilevel frameworks both to holonomic [74,75] and nonholonomic planning problems [97,67]. To create multilevel abstraction, we can often remove links from a robot [5,103], shrink links [3,78] or approximate a robot by simpler geometries, either exact [65,31] or approximate [11,76,93]. While most methods use prespecified levels of abstraction, we can also use workspace information to compute abstractions on the fly [101,56], adaptively switch between abstractions [89] or learn useful abstractions for specific instances [9]. Our approach is similar, in that we also use a multilevel motion planning framework [67]. However, our work is complementary, in that we focus specifically on computing path sections in the presence of narrow passages in the state space.
A second source of admissible heuristics are lazy search [8,33] and informed sets [26,44]. Instead of using relaxations, we can compute lazy paths (paths not checked for collisions), either forward from the start [36] or backwards from the goal [88], to create an efficient heuristic which we can exploit using dedicated algorithms [27]. Once a solution exists, we can also exploit informed sets, sets which exclude all states with provable higher cost-to-go [26,27]. Those methods are particularly important, since edge evaluations is one of the bottlenecks in motion planning [47]. It makes therefore sense to develop heuristics which evaluate edges as late as possible [60,37].
Third, inspired by pattern database approaches in discrete search [15,21,39], we can also construct admissible heuristics by using past experience. We can achieve this by either precomputing motion primitives, like steering functions or controllers like linear quadratic regulators [80,79]. Or, we can store previous solution paths directly and use them as heuristics in new environments [17,73]. Our work is complementary in that we assume a heuristic given and we focus on exploiting this heuristic as efficiently as possible.

B. Exploiting Admissible Heuristic
Given an admissible heuristic, we can optimally exploit it by discretizing the state space [23] and by using the A* algorithm [34,70,1]. However, discretizing the state space usually does not scale well to higher dimensional state spaces [12,71,28] and performance would be sensitive to the resolution used [19]. To avoid discretization, we found three categories of works which use continuous methods to exploit admissible heuristics.
First, we can use biased sampling methods. A straightforward way would be to represent the heuristic value of a state by the radius of a hypersphere around the state [54]. We could then exploit this hypersphere using dynamic domain sampling [100]. Using such a scheme, we would expand states with higher heuristic values more often. Depending on the exact type of heuristic function used, we would obtain sampling distributions which would increase the probability to sample states which are near to restricted workspace geometries [94,99], to state space obstacles [2] or to narrow passage [38]. Those sampling distributions could also be learned over time to improve sampling [57,41]. Our approach is similar in that we also use sampling-based methods. We differ, however, in that we concentrate on designing efficient patterns complementary to biased sampling methods.
Given a solution to a relaxed problem, we can often use this solution as a guide path heuristic [103,93] to quickly find a solution in the original state space. Using the parlance of fiber bundles, we refer to this approach as the path section approach [67]. To implement a path section approach, we can often use restriction sampling [69,65]. Restriction sampling is often used in high-dimensional contact planning cases [10,93], where uniform sampling would most likely fail to find solutions in a reasonable time [31]. Apart from biasing sampling, we can also explicitly search over the set of states which project onto the guide path (the path restriction) [103]. One particular method is the sidestepping path section approach [67], where we propagate states while following the path restriction. Once collisions occur, we execute local sidesteps to move around the obstacle. However, as we show in Sec. IV, sidesteps are often not beneficial for narrow passages. While we also use a path section approach, we differ by developing dedicated patterns to more efficiently traverse narrow passages.
Path section approaches and other heuristic search methods often fail because they reach local minima. We define a local minimum as a region in state space where the heuristic is not or only weakly correlated with the true cost-to-go [96]. To address local minima, we can choose one of two approaches. First, we could preemptively avoid local minima. If the environment is static, we can learn minima regions and use this information to update the heuristic function [96]. Second, we could try to escape local minima. There exist several methods to escape local minima like deflating the heuristic value of states close to obstacles [18] or increasing the search resolution to prevent evaluation of closeby states [19]. A related idea is to utilize Tabu search [29] to prevent sampling in previously visited regions.
It is important to make the distinction between local minima which trap the planner and regions which might look like local minima but which a planner can actually traverse. We call such regions narrow passages [83]. To verify the existence of narrow passages in low-dimensional state spaces, we can use exact infeasibility proofs [84,4], for example using geometrical shapes like alpha complices [61] or cell decomposition methods [102]. Because many state spaces have a local product structure, we can often use configuration space slices [55,86] to efficiently test for infeasibility [95]. If the problem is feasible, we could then use the geometrical shapes to enumerate narrow passages [59]. To exploit narrow passages, we could bias sampling to the most constricted areas [99,92]. We differ to those approaches by not explicitly modeling narrow passages or local minima, but we instead develop reactive measures to escape minima and to traverse narrow passages. We thereby avoid spending time on irrelevant narrow passages.

III. BACKGROUND
Let us describe the necessary background to follow the exposition of our algorithm in Sec. V and Sec. IV. We start by explaining multilevel motion planning, i.e. planning with sequences of relaxed subproblems. While several formulations exist, we believe the framework of fiber bundles [67] to be a good way to concisely model multilevel abstractions and describe our algorithms. We then describe the concepts of lift, path restriction and path section which are particularly important. Finally, we describe the notion of admissible heuristics, which is one of the fundamental concepts to exploit solutions to relaxed problems.

A. Optimal Motion Planning
Let X be the state space of the robot. To each state space we associate a constraint function φ : X → {0, 1} which evaluates to 0 if a state is constraint-free and to 1 otherwise. We use the constraint function to define the free state space X F = {x ∈ X | φ(x) = 0}. Together with an initial configuration x I ∈ X F and a goal configuration x G ∈ X F , we define an optimal motion planning problem [45,81,6] as the tuple (X F , x I , x G , c), whereby our task is to develop an algorithm which computes a path from x I to x G while staying in X F and minimizing the cost functional c. In this work, we use a minimal-length cost functional, but other costs are also possible like minimal energy or maximum clearance.

B. Multilevel Motion Planning
Since high-dimensional motion planning problems are often too computationally expensive to solve, we use a sequence of relaxed problems which we refer to as multilevel abstractions [67]. Given a state space X, let us denote a multilevel abstraction as the tuple {X 1 , · · · , X K } with X K = X. To each state space X k , we associate a constraint function φ k and a projection π k from X k to X k−1 . We say that the projection π k is admissible (w.r.t. the constraint functions), if we basically guarantee that solutions are preserved under projections [63]. If we would allow inadmissible projections, we would potentially sacrifice solutions and thereby sacrifice (probabilistic) completeness.

C. Fiber Bundle Formulation
When working with multilevel abstraction, we quickly stumble upon situations where we lack the appropriate vocabulary to describe solution strategies. As a remedy, we describe multilevel abstractions using the framework of fiber bundles [87,40,52]. A fiber bundle is a tuple (X k , X k−1 , F k , π k , π F k , consisting of a total space X k , a base space X k−1 , a fiber space F k , a projection mapping π k from total to base space and a fiber projection mapping π F k from total to fiber space. We assume the projection mapping π k to be admissible. With a fiber bundle, we model product spaces which locally decompose as X k = X k−1 × F k . The total space X k is a union of fiber spaces which are parameterized by the base space X k−1 . We show a prototypical fiber bundle in Fig. 2 (left). If the level k is unimportant for the task as hand, we often refer to a fiber bundle as the tuple (X, B, F, π, π F ) with X being the total, B the base, F the fiber space and π, π F the base and fiber projection, respectively. For more details and motivation, we refer to our prior work [67]. For the purpose of this paper, we focus on the three concepts of lift, path restriction and path section, which we explain next.

D. Lift
Let (X, B, F, π, π F ) be a fiber bundle and let b ∈ B be a base space element. We often like to project the element b back to the total space X. We call this operation a lift [77,67]. We define a lift as a mapping LIFT : B → X. To uniquely select an element in X, we will overload this function as a mapping LIFT : B × F → X by providing a fiber space element f in F . If X is a product space, we define the lift as

E. Path Restrictions
Let p : I → B with I = [0, 1] be a path on the base space (a base path). Given a base path, one of the most central sets which we use in this work are path restrictions. A path restriction is the set is the image of the base path in B and π is the projection from X to B. We visualize this situation in Fig. 2 (right), where we show the image of a base path on the disk-shaped base space and its associated path restriction on the total space.

F. Path Sections
Given a path restriction, we are often interested in finding paths which are lying inside the path restriction. We call them path sections [87]. A (smooth) path section w.r.t. a base path p is a mapping s from base space to total space such that π(s(u)) = u for any u in the image of p [52]. This means, for each base path element, we select a unique state from the path restriction.

G. Admissible Heuristics
Our motivation to introduce path restrictions and path sections comes from the role they play in exploiting admissible heuristics. Given a goal state x G , an admissible heuristic h(x) for a state x in X is a lower-bound on the true cost-to-go (or value) function h * (x), which we define as the cost of the optimal path from x to x G through X F . Formally, we write this condition as h(x) ≤ h * (x) [70,1,63].
Given an admissible heuristic, we can try to reach the goal x G by using locally optimal decisions [34]. If we are at a state x, we can make an optimal decision by doing a two-step approach. First, we compute the f -value of all its neighbors, which is the sum of its heuristic value and its cost-to-come from the start state. We then expand the state (node) with the lowest f -value, because, under the admissible heuristic, it is our best guess to efficiently reach the goal [70].
However, in a continuous domain, we cannot straightforwardly compute all neighboring states. Instead, we imagine computing a small -neighborhood around the state. To compute heuristic values, we project the complete neighborhood down onto the base space. To reach the goal, our best guess is to make a step into the direction of the current minimalcost base path. The states which we would expand in that way are exactly the states on the path restriction. By searching a path section over this path restriction, we efficiently exploit the admissible heuristic given by the base path.

IV. SECTION PATTERNS
In this section, we formulate the problem of finding sections over path restrictions. To find sections, we develop four section patterns to efficiently traverse narrow passages and escape local minima. A local minimum is defined as a region where the true cost is only weakly correlated with the true cost-togo [96]. We eventually integrate the section patterns into the pattern dance algorithms we present in Sec. V.

A. Problem Formulation
Let X be a state space with initial state x I ∈ X and goal state x G ∈ X. Further, let (X, B, F, π) be a fiber bundle on X (possibly in a sequence of fiber bundles) and let p : I → B be a base path on B starting at π(x I ) and ending at π(x G ). Given the base path p and its path restriction r(p) ⊆ X, our goal is to develop an algorithm to find a feasible path section, i.e. a path lying in the intersection of the path restriction r(p) and the free state space X F connecting x I to x G .

B. Head Pointer on Path Restriction
For better exposition of our algorithm, we will introduce the terminology of a head pointer on a path restriction. We define a head pointer h as the tuple h = (x, l, r) consisting of a path restriction r(p) ⊆ X over a base path p in B, a current state x in r(p) and a location l ∈ [0, 1] defining the position along the base path. We think of the head pointer as a ruler which we try move forward along the path restriction towards the goal state. We visualize this situation in Fig. 3. In State p(l) on base path 7: Return Last Valid 13: return HASREACHEDGOAL(h) pseudocode, we refer to the current state as STATE(h) and its location as LOCATION(h).
The role of the section patterns is to propagate the head pointer forward along the path restriction. With propagating the head pointer we refer to the action of increasing the location of the head along the base path. All section patterns implement different strategies to accomplish this while keeping the current state inside of the free space X F .

C. Manhattan Pattern
Our first section pattern to propagate the head pointer h is the Manhattan (MH) pattern. With the MH pattern, we interpolate a path between the head state and the goal state along the path restriction. To interpolate, we first interpolate along the base path while keeping the fiber element fixed. Once we reach the end of the base path, we interpolate along the fiber space to the goal state. This method is motivated by our desire to actuate the smallest number of joints at the same time, which is advantageous for high-dimensional systems [14].  We detail the MH pattern in Alg. 1. We take as input a head pointer h over a path restriction r with base path p. We first project the head state onto the fiber (Line 1-2) by using the fiber projection π F . We then take the location of the head pointer along the base path (Line 3) and step along the base path in increments of δ X k−1 (Line 5-10) and add the states to the path s (Line 4). This is done by computing the next base state (Line 6), lifting the base state into the total space (Line 7) and adding it to the path (Line 8). Once we reached the end of the base path, we add the goal state to the section (Line 11). The resulting path s is schematically shown in Fig. 3. Finally, we evaluate the path by moving along until a constraint violation occurs or we reached the goal state (Line 12). The function CHECKMOTION returns the last valid state which we use to update the head h. We then return true if the head has reached the goal and false otherwise.

D. Interlude: The Geometry near Narrow Passages
The next three section patterns are tailor-made solutions to either traverse a narrow passage or to escape a local minimum. To motivate those patterns, we first study the geometry of state spaces near narrow passages. We use a simple toy example of a rigid rectangular body moving in the 2D plane. The state space of this rigid body is the special euclidean group SE(2), consisting of position and orientation. We assume that the body is located near to a narrow passages as shown in Fig. 4 (left). We will further assume that our task is to move the rigid body through the narrow passage, from a start state (green) to a goal state (red). We will represent a state as (x 0 , x 1 , x 2 ) ∈ SE(2), with x 0 , x 1 being vertical and horizontal displacement and x 2 the orientation. We visualize a subset of the state space in Fig. 4 (right), whereby points in collision are colored from dark red (low x 1 value, close to start) to bright blue (high x 1 value, close to goal).
To generate path restrictions, we first use a relaxation of the problem onto circular disks (Fig. 4). This creates a fiber bundle   Fig. 4 (right). From Fig. 5, we observe that there are at least three failure cases. Either, we reach a local minimum, we collide with constraints near local minima or narrow passages, or we get stuck in front of a small but infeasible region. For each case, we develop a dedicated section pattern to either advance or backtrack.

E. Triple Step Pattern
To escape a local minimum, we develop the triple step pattern. With the triple step pattern, we connect two states on the path restriction using a triple backtracking step. For the rectangular rigid body in the plane, we visualize this situation in Fig. 6. We use the triple step pattern to connect states p 1 and p 4 . To do that, we move backwards along the path restriction from p 1 to p 2 and from p 4 to p 3 , respectively. We stop until we can connect p 2 and p 3 by a straight line. In that case we execute a backstep from p 1 to p 2 , a sidestep (along the fiber) from p 2 to p 3 and a forward step from p 3 to p 4 .
We show the pseudocode for the triple step pattern in Alg. 2. Our goal is to connect the head state to the given state x.
We first compute a midpoint on the fiber space (Line 5) (to minimize the number of CHECKMOTION calls [60]). We then move backwards along the base path while we are greater than the parameter δ X k−1 (Line 6-7). For each location, we interpolate a base state (Line 8), lift the state using the fiber midpoint (Line 9) and check if this state is valid. If it is valid, we compute intermediate states x 1 and x 2 (Line 11, 12) and if ISVALID(x mid ) then 11: 12: if CHECKMOTION(x 1 , x 2 ) then 14: if CHECKMOTION(x H , x 1 ) then 15: if CHECKMOTION(x 2 , x) then 16:  (Line 14, 15). If that is true, we add those edges to the graph (Line 16-18) and update the head to our new state x (Line 19). In that case we return true (Line 20). If we fail to find such a triple step, we terminate once we reach the beginning of the base path location and return false (Line 27).

F. Wriggle Pattern
If we reach a local minimum, the triple step pattern is a way to backtrack to a narrow passage. However, we often might execute the triple step pattern prematurely, because we bumped into constraints near or in a narrow passage. To circumvent those situations, we use the wriggle pattern. With the wriggle pattern, we make small random steps forward along the path restriction and accept a step if it is valid.
The pseudocode we depict in Alg. 3. We start by making one δ X k−1 step forward from the head (Line 1). Until we have not reached the end (Line 3), we get the base state at location l (Line 4), and get the fiber element of the head state (Line 6). We then sample for S max rounds (Line 8) by sampling a fiber state in the δ F k proximity of the head fiber state (Line 9). We then lift the base and fiber state (Line 10) and check if the state is valid (Line 11). If the state is valid, we check if the motion from the head to the new state is feasible (Line 12-17). We terminate if we could not expand the state (Line Algorithm 3 WrigglePattern(h) 1: l ← LOCATION(h) + δ X k−1 2: steps ← 0 3: while l < LENGTH(p) do 4: x B ← BASEPATHAT(p, l) 5: x h ← STATE(h) 6: x Fh ← PROJECTFIBER(x h ) 7: ctr ← 0 8: while ctr < S max do 9: x F ← SAMPLEUNIFORMNEAR(x Fh , δ F k ) 10: x ← LIFT(x B , x F ) 11: if ISVALID(x) then 12: if CHECKMOTION(x H , x) then 13:

G. Tunnel Pattern
While the wriggle pattern locally explores the neighborhood inside the path restriction, we often encounter situations where we find it advantageous to momentarily step outside the path restriction to overcome an infeasible region. From the perspective of the path restriction, we "tunnel" through the infeasible region, which we therefore refer to as the tunnel pattern. With the tunnel pattern, we assume to be located at a local minimum p 1 as shown in Fig. 7. To resolve this situation, l ← l + δ X k−1 13: x B ← BASEPATHAT(p, l) 14: ← SMOOTHPARAMETER(0, 10δ X k−1 , S max ) 15: ctr ← 0 16: while ctr < S max do 17: x B ← SAMPLEUNIFORMNEAR(x B , (ctr)) 18: if d < d best and CHECKMOTION(x h , x) then 23: we try to find the next valid state p 2 while keeping the fiber element constant. We then try to connect p 1 to p 2 by sampling valid states in a smoothly increasing neighborhood of the base space and a constant neighborhood in fiber space. While p 2 is not reached, we accept new states if they decrease the distance to p 2 .
We show the pseudocode in Alg. 4. We first search for a tunnel ending state x End at base path location l End (Line 1). To find the tunnel ending, we step forward along the base path without changing the fiber until we find a valid state. We then try to connect the head state x h to the tunnel ending state x End . We use a while loop to move along the relevant base path segment from the head location l to the tunnel end location l End (Line 6). We first check if we can connect the head state to the tunnel end state (Line 7). If true, we add a new edge into the graph (Line 8), set the head to the tunnel ending state (Line 9) and return true (Line 10). Otherwise, we step forward along the base path with step size δ X k−1 (Line 12) and query the base state at l (Line 13). Instead of using the base state exactly, we use a smoothly increasing Algorithm 5 MultilevelPlanner(x I , x G , X 1 , · · · , X K ) 1: Let X be a PRIORITY QUEUE 2: for k = 1 to K do 3: FINDSECTION(X k ) 4: X.PUSH(X k ) 5: while ¬PTC(X k ) do r ← RESTRICTION(p) 4: h ← HEADPOINTER(x I , location = 0, r) 5: PATTERNDANCE(h) 6: end if neighborhood parameter . The value of depends on the counter CTR and smoothly interpolates between 0 and 10δ X k−1 using an Hermite polynomial [16] (Line 14). We then attempt to make a step towards the tunnel ending for a maximum of S max attempts (Line 16). We do this by sampling a base space element (Line 17) and a fiber element (Line 18). We then lift the state (Line 19) and check for validity (Line 20). If the new state is valid, its distance is closer to the tunnel ending and we can connect it to the head state (Line 22), we add a new edge to the graph (Line 23), set the head state to the new state (Line 24) and continue forward (Line 25). If we fail to find a better sample for S max attempts, we return false (Line 30-32). We also return false if we reach the base path location l End without having a valid connection (Line 34).

V. PATTERN DANCE ALGORITHM
To combine and coordinate section patterns, we develop the pattern dance algorithm. The pattern dance algorithm is a recursive algorithm which takes as input a path restriction over a base path, a start and a goal configuration, and either returns a feasible path section or terminates after a certain depth has been reached. We use the pattern dance algorithm in combination with the multilevel planner QMP, QMP*, QRRT and QRRT* [67] to quickly determine if a given base path has a feasible section. The resulting algorithms inherit all properties from PRM [46], PRM* [45], RRT [48] and RRT* [45], respectively, i.e. they are probabilistically complete, meaning the probability of finding a feasible path if one exists converges to one if time goes to infinity. The star versions are asymptotically optimal, meaning they will eventually converge to an optimal solution if time goes to infinity [45,67].

A. Pattern Dance as Part of Multilevel Planner
Before describing the pattern dance algorithm, we describe how it is embedded in the larger context of multilevel planner. A multilevel planner [67] computes a feasible path for a multilevel motion planning problem. We describe such an algorithm in Alg. 5. We initialize the algorithm with an initial state x I , a goal state x G and a sequence of bundle spaces X 1 , . . . , X K . To search for a feasible path, we first initialize a priority queue (Line 1), then we iteratively explore the bundle spaces (Line 2) by first testing for a path section (Line 3) and pushing the k-th bundle space into the priority queue (Line 4). We then loop while a planner terminate condition (PTC) of the k-th space is not fulfilled. A PTC can be a timelimit, an iteration limit or a desired cost. We then pop the space with the lowest importance from the queue (Line 6), which depends on the algorithm itself [67]. We then execute one grow iteration for the selected bundle space (Line 7) and push the space back to the queue thereby updating its importance (Line 8). All multilevel planner share this high-level structure. Multilevel planner differ by how the GROW function is implemented.
We previously developed four multilevel planner. First, the quotient-space roadmap planner (QMP), in which we implement GROW as a probabilistic roadmap (PRM) step [46]. Second, the quotient-space rapidly-exploring random tree (QRRT), in which we implement GROW as an RRT step [48]. Finally, we use the two asymptotically optimal versions QRRT* and QMP*, in which we implement a step of RRT* and PRM* [45], respectively. The algorithms also differ in how we order the bundle spaces inside the priority queue, how we compute the distance metric and how we implement sampling inside the grow function, as we detail in our previous publication [67].
We embed the pattern dance algorithm in the multilevel planner as a particular implementation of the FINDSECTION method. We show this in detail in Alg. 6. First, we check if there exists a base space (Line 1). We then compute a base path p from the underlying graph or tree on the base space (Line 2). We then build a path restriction r from p (Line 3) and create a head on the path restriction (Line 4). We then call the pattern dance algorithm with the head as input.

B. Pattern Dance Implementation
We depict the pseudocode of the pattern dance algorithm in Alg. 7. The input is a head over the path restriction and a recursion depth (initially set to zero). We first execute the MANHATTANPATTERN (Line 1). If the pattern succeeds, we successfully return (Line 2). Otherwise, we check if we reached the maximum recursion depth (Line 4) and return with failure (Line 5).
If the depth is below the maximum depth, we continue by executing first the WRIGGLEPATTERN and the TUNNEL-PATTERN (Line 7). If one is successful, we recursively call the pattern dance algorithm and we increase the recursion depth (Line 8). If none is successful, we are likely in a local minimum from which we like to escape using the TRIPLESTEPPATTERN. To execute the triple step pattern, we first interpolate a single step forward along the base path (Line 10, 11). We then attempt to find a valid fiber space element for a maximum of S max attempts (Line 12). This is done by first sampling a fiber state over the given base state (Line 13). We then lift the state to the path restriction (Line 14) to obtain return PATTERNDANCE(h, depth+1) 9: end if 10: l ← LOCATION(h) + δ X k−1 11: x B ← BASEPATHAT(p, l) 12: for j ∈ [1, B max ] do 13: 14: x ← LIFT(x B , x F ) 15: x H ← STATE(h) 16: if ISVALID(x) and ¬CHECKMOTION(x H , x) then 17: if TRIPLESTEPPATTERN(h, x) then Maximum branching of pattern dance 500 Smax Maximum sampling attempts 100 δ X k−1 Step size on base space Step size on fiber space 0.01µ F k a state x. If this state is valid and we cannot reach it from the head state (Line 16), we execute the triple step pattern with target x (Line 17). If we successfully executed the pattern, we call the pattern dance algorithm again recursively. Note that the small forward step of δ X k−1 (Line 10) is an essential component of our algorithm. If we would sample directly over the head base state, we often would sample symmetrical local minima (as an example, see state p 1 in Fig. 6). we found this to be particularly important for higher dimensional state spaces, where we often encounter infinitely many symmetrical local minima (consider the set of horizontal rotations of the cylinder before entering the opening in the Bugtrap scenario in Sec. VI).
To implement the section patterns and the pattern dance algorithm, we use the open motion planning library (OMPL) [91]. The algorithms are freely available and part of our multilevel motion planning extension of OMPL [67]. All code can be downloaded over github 1 . All parameters used in the algorithms are shown in Table I Fig. 8, each averaged over 10 runs with cut-off time limit of 60s. An entry − means that planner does not support the particular state space.
To evaluate our pattern dance algorithm, we integrate it as an elementary check into the multilevel planner QRRT, QRRT*, QMP and QMP*. We then conduct two comparisons. First, we compare our planner to 36 available planning algorithms in the Open motion planning library (OMPL) [62] on 7 challenging environments as shown in Fig. 8. For each algorithm, we will   use the abbreviated name. For a full list of algorithms with full names and associated publication, see [67] and the OMPL documentation [91]. Second, we compare the multilevel plan-ner with the pattern dance algorithm to an older version of the same multilevel planner, where we use a recursive sidestepping algorithm to quickly find sections [67].

A. Evaluation Metric
To evaluate, we use a 8GB RAM 4-core 2.5GHz laptop running Ubuntu 16.04. For each experiment, we use a minimum length cost (for planner which support cost functions) and we let each planner run 10 times with a cut-off time limit of 60 seconds. We then report on the average runtime over those 10 runs. We show the results in Table II.
Concerning the results, there are two notes of caution. First, we let each OMPL planner run out-of-the-box without any parameter tuning. Further tuning of parameters could potentially improve results significantly. Second, due to the high number of planner and scenarios, we let each planner run only 10 times and take the average. However, averaging over 10 runs might exhibit more variance and thereby create more outlier.

B. 06-dof Bugtrap
For the first evaluation, we use the Bugtrap scenario [53] (Fig. 8a). The lowest runtime we found in the literature is 22.17s for a version of the Selective-Retraction-RRT [53,102].    However, this runtime is not directly comparable due to different hardware, implementation, parameters and operating systems. To relax the problem, we use an inscribed sphere at the center of the cylindrical bug as shown in Fig. 9g and Fig. 9d.
We show the results of our evaluation in Fig. II. The best performing planner is QMP (3th planner in table) with 0.51s followed by QMP* (4) with 0.90s and QRRT (1) with 4.45s. We also see good performance of the BiTRRT (13) planner [43] with 11.54s. We note that the QRRT* (2) algorithm requires 24.87s, which we believe to be caused by the additional burden of rewiring the tree [82,67].

C. 06-dof Double L shape
In the next evaluation, we like to show that the section patterns are not specific to the cylindrical geometry, but are more widely applicable to other rigid bodies. As demonstration, we use the double L-shape scenario [94], where two Lshape bodies are connected to each other as shown in Fig. 8b. The task is to move through a vertical wall with a small quadratic hole. We use a two-level relaxation by using an inscribed sphere as shown in Fig. 9h and Fig. 9e. To make our method more robust against base paths too close to obstacles, we increase the size of the sphere slightly to increase clearance from obstacles.

D. 10-dof Chain Egress
In the third evaluation, we like to increase the complexity by considering an articulated chain (10-dof) as shown in Fig. 8c. The task is to remove the chain from a pipe, a typical egress scenario. Note that for such systems, we can find analytical feasible path sections if we assume the base path of the head to be curvature constrained [66]. However, we will not make such assumption in this paper.
To relax the problem, we use an inscribed sphere in the head of the chain as shown in Fig. 9i and Fig. 9f. As in the case of the double L-shape, we slightly increase the size of the sphere to make our method more robust against base paths too close to obstacles.
In our evaluations, we show that QRRT performs best with 0.55s followed by QRRT* (0.56s). The next best planners are TRRT (11) (0.81s), QMP (1.91), BiTRRT (12) (4.57s) and QMP* with 7.29s. Note that there are 12 OMPL planner which cannot address this problem, because they do not support compound state spaces or do not have dedicated projection functions for such spaces.

E. 37-dof Pre-Grasp
For the next evaluations, we compute (pre-)grasping paths for a ShadowHand mounted on a KUKA LWR robot. The tasks are to compute an overhand grasp on a ball (Fig. 8d), an underhand grasp on a metal piece (Fig. 8e), a singlefinger precision grasp on a mug (Fig. 8f) and a double-finger precision grasp on a scissor (Fig. 8g). The starting state for all scenarios is an upright position of the arm with hand being open, as shown in Fig. 9a. To relax the problem, we use a three-level abstraction by first removing three fingers (Fig. 9b) and subsequently removing the thumb (Fig. 9c) of the hand.
Our evaluations show the following results. First, for the Ball scenario, we see that QMP and QMP* perform best with 0.86s. The next best planner is the OMPL planner BiRLRT (15) [56] with 1.52s, QRRT with 2.01s and RRTConnect (6) with 1.70s. We note that also the planner PDST (35) [50], RLRT (14) [56] and KPIECE1 (36) [90] perform competively with 3.25s, 3.68s and 6.27s, respectively. The planner QRRT* does not perform well on this problem instance with 25.35s, due to similar problems as on the Bugtrap scenario. Second, for the underhand grasp on the metal piece, we see that QMP* performs best with 1.94s followed by RRTConnect (6) with 8.16s and QMP with 18.98s. We will address the discrepancy between QMP and QMP* further in Sec. VII. Third, for the single-finger precision grasp on the mug, we observe that QMP performs best with 1.20s followed by QMP* with 1.63s. While QRRT performs significantly worse (19.80s), QRRT* was not able to solve this problem (60.00s). Fourth, for the doublefinger precision grasp on the scissor, we observe that QMP performs best with 14.52s followed by QMP* with 37.27s. No other planner is able to solve this problem. We will further

VII. LIMITATIONS AND DISCUSSION
While our evaluations support the usage of section patterns for narrow passage planning problems, we also like to point out two limitations of our approach. To each limitation, we will discuss possible ways to eventually address and resolve the limitation.

A. Increased runtime on Metal and Scissor Scenario
The first limitation is the increased runtime of our planner on the 37D ShadowHand Scissor and the Metal scenario. We distinguish between two subproblems. First, we observe that QRRT and QRRT* have a runtime of 60s on the Scissor scenario. Both scenarios, however, are ingress scenarios, where the planner needs to find a narrow passage on the base space to enter the goal region, which is challenging for RRT-like algorithms [48] and could be addressed using a bidirectional version of QRRT.
Second, we observe that QMP and QMP* require 14.52s and 37.27s to solve the Scissor scenario and that QMP requires 18.98s to solve the Metal scenario. To explain this rather large increase in runtime, we have a closer look at the individual runtimes, which we show in Table IV. We can observe that both planner exhibit one of two outputs. Either, they quickly return a solution (usually less than 3s, always less than 10s) or they fail and time out at 60s (three/two times for QMP, zero/six times for QMP*). To us, this indicates that both algorithms might be sensitive to the base space path. If the base path is not smooth enough, has kinks in it, or is too close to obstacles, then we might not be able to solve it with the pattern dance algorithm. We could address this problem in the future by either additional smoothing of the base space path [97], by introducing conservative heuristics [13] or by switching to a different relaxed model [89].

B. Base path does not admit a feasible section
While all multilevel planner are probabilistically complete, we often need the pattern dance algorithm to efficiently solve a problem. However, we might encounter scenarios, where the base path does not admit a feasible path section. Such a situation is shown in Fig. 10. The scenario depicts an Xshape robot, which has to traverse a shape-sorter box with different openings, which we relax by inscribing a sphere (right). Planning for the spherical robot might produce a base path going through the wrong hole. Such a base path does not admit a feasible path section, meaning there are no paths along the path restriction of the base path to traverse towards the goal. While multilevel planner are probabilistically complete and would eventually resolve the situation, we would not be able to do it efficiently using our pattern dance algorithm. To address such situations, we could either compute several base paths [64,32,98,68,7,72] or we could automatically choose an alternative relaxation using either a meta-heuristic [9] or a brute-force search [63].

VIII. CONCLUSION
We developed the pattern dance algorithm, which takes as input a base space path and efficiently exploits its path restriction using the section patterns Manhattan, wriggle, tunnel and triple step. We showed in evaluations, that our pattern dance algorithm successfully coordinates section patterns and outperforms a similar sidestepping algorithm [67]. We then showed that multilevel motion planning algorithms using our pattern dance algorithm outperform classical planner from the OMPL library on challenging narrow passage scenarios including the Bugtrap, chain egress and precision grasping. With some exceptions, we often observed runtime improvement by one to two order of magnitudes.
While we demonstrated to efficiently solve narrow passage problems, we also pointed out two limitations. First, we observe an increased runtime in some planning instances. We could address this problem by either further deforming the base path [103], by improved neighborhood modeling [49] or by learning of the section patterns themselves [41]. Second, we cannot find handle cases where the base path does not admit path sections. We could address this problem by computing multiple base paths [64,68,98] or using more informed graph restriction sampling methods [63].
Despite limitations, we believe to have contributed a novel solution method which we can use to efficiently find sections over base path restrictions. We believe our method to be a promising tool to further probe, understand and efficiently exploit high-dimensional state spaces.